Add stationary relay block commands

LocCorr/basler.c

@@ -111,7 +111,7 @@ static int getBoolean(const char *featureName, _Bool *val){
     if(!chkNode(&hNode, featureName, BooleanNode, FALSE)) return FALSE;
     if(!val) return TRUE;
     PYLONFN(GenApiBooleanGetValue, hNode, val);
-    DBG("Get boolean: %s = %s", featureName, val ? "true" : "false");
+    //DBG("Get boolean: %s = %s", featureName, val ? "true" : "false");
     return TRUE;
 }
 static int getInt(char *featureName, int64_values *val){
@@ -123,7 +123,7 @@ static int getInt(char *featureName, int64_values *val){
     PYLONFN(GenApiIntegerGetMax, hNode, &val->max);
     PYLONFN(GenApiIntegerGetInc, hNode, &val->incr);
     PYLONFN(GenApiIntegerGetValue, hNode, &val->val);
-    DBG("Get integer %s = %ld: min = %ld, max = %ld, incr = %ld", featureName, val->val, val->min, val->max, val->incr);
+    //DBG("Get integer %s = %ld: min = %ld, max = %ld, incr = %ld", featureName, val->val, val->min, val->max, val->incr);
     return TRUE;
 }
 static int getFloat(char *featureName, float_values *val){
@@ -134,7 +134,7 @@ static int getFloat(char *featureName, float_values *val){
     PYLONFN(GenApiFloatGetMin, hNode, &val->min);
     PYLONFN(GenApiFloatGetMax, hNode, &val->max);
     PYLONFN(GenApiFloatGetValue, hNode, &val->val);
-    DBG("Get float %s = %g: min = %g, max = %g", featureName, val->val, val->min, val->max);
+    //DBG("Get float %s = %g: min = %g, max = %g", featureName, val->val, val->min, val->max);
     return TRUE;
 }
 

LocCorr/improc.c

@@ -122,7 +122,7 @@ static void getDeviation(object *curobj){
     Xc[counter] = curobj->xc; Yc[counter] = curobj->yc;
     if(fXYlog){ // make log record
         fprintf(fXYlog, "%.2f\t%.1f\t%.1f\t%.1f\t%.1f\t%.1f\t",
-                dtime() - tstart, curobj->xc, curobj->yc,
+                dtime(), curobj->xc, curobj->yc,
                 curobj->xsigma, curobj->ysigma, curobj->WdivH);
     }
     //DBG("counter = %d", counter);

LocCorr/improc.h

@@ -24,7 +24,7 @@
 #include "imagefile.h"
 
 // tolerance of deviations by X and Y axis (if sigmaX or sigmaY greater, values considered to be wrong)
-#define XY_TOLERANCE                (1.)
+#define XY_TOLERANCE                (5.)
 
 #define PUSIROBO_POSTPROC   "pusirobo"
 // how many frames will be averaged to count image deviation
@@ -42,6 +42,7 @@ typedef struct{
     char *(*movefocus)(const char *newstatus, char *buf, int buflen);
     char *(*moveByU)(const char *val, char *buf, int buflen);
     char *(*moveByV)(const char *val, char *buf, int buflen);
+    char *(*relay)(const char *val, char *buf, int buflen);
     void (*stepdisconnect)();
 } steppersproc;
 

LocCorr/pusirobo.c

@@ -17,6 +17,7 @@
  */
 #define _GNU_SOURCE
 #include <arpa/inet.h>
+#include <inttypes.h>
 #include <math.h>
 #include <netdb.h>
 #include <pthread.h>
@@ -45,6 +46,22 @@
 #define registerUaxe        "register U 0x581 stepper"
 #define registerVaxe        "register V 0x582 stepper"
 #define registerFocus       "register F 0x583 stepper"
+#define registerRelay       "register R 1 raw"
+#define RelayCmd            "mesg R 1"
+#define RelayAns            "#0x001"
+static const int relaySetter = 0x80; // add this to command of setter
+// relay commands:
+typedef enum{
+    R_PING = 0,
+    R_RELAY,
+    R_PWM,
+    R_ADC,
+    R_MCU,
+    R_LED,
+    R_BTNS,
+    R_TIME,
+    R_ERRCMD
+} relaycommands;
 #define setUspeed           "mesg U maxspeed 22400"
 #define setVspeed           "mesg V maxspeed 22400"
 #define setFspeed           "mesg F maxspeed 12800"
@@ -96,6 +113,13 @@ typedef enum{
 } setupstatus;
 static _Atomic setupstatus sstatus = SETUP_NONE; // setup state
 
+typedef struct{
+    uint8_t relays;
+    uint8_t PWM[3];
+    uint8_t buttons[4];
+} relaystate;
+static _Atomic relaystate relay;
+
 static pusistate state = PUSI_DISCONN;   // server state
 // the `ismoving` flag allows not to make corrections with bad images made when moving
 static volatile atomic_bool ismoving = FALSE; // == TRUE if any of steppers @hanging part is moving
@@ -260,6 +284,53 @@ static int send_message(const char *msg, char **ans){
     return r;
 }
 
+/**
+ * @brief getRansArg - check relay answer & return args
+ * @param ans - answer
+ * @param buf - full buffer
+ * @return amount of args found (0 - if answer is wrong or no n'th arg found)
+ */
+static int getRansArg(char *ans, uint8_t buf[8]){
+    //DBG("check relay answer, ans: %s", ans);
+    if(!ans) return 0;
+    if(strncmp(ans, RelayAns, sizeof(RelayAns)-1)) return 0; // bad answer
+    ans += sizeof(RelayAns);
+    int got = sscanf(ans, "%hhx %hhx %hhx %hhx %hhx %hhx %hhx %hhx", &buf[0], &buf[1], &buf[2], &buf[3], &buf[4], &buf[5], &buf[6], &buf[7]);
+    //DBG("got ans: %d, arg0..2=%u, %u, %u", got, buf[0], buf[1], buf[2]);
+    return got;
+}
+
+/**
+ * @brief chkRelay - check relay state & change `relay` variable
+ * @return FALSE if failed
+ */
+static int chkRelay(){
+    char *ans = NULL;
+    char buf[512];
+    uint8_t canbuf[8];
+    relaystate r = {0};
+    int ret = FALSE;
+    snprintf(buf, 511, "%s %d", RelayCmd, R_RELAY);
+    if(send_message(buf, &ans) && 2 == getRansArg(ans, canbuf) && canbuf[0] == R_RELAY){
+        r.relays = canbuf[1];
+    }else goto rtn;
+    snprintf(buf, 511, "%s %d", RelayCmd, R_PWM);
+    if(send_message(buf, &ans) && 4 == getRansArg(ans, canbuf) && canbuf[0] == R_PWM){
+        memcpy(r.PWM, canbuf+1, 3);
+    }else goto rtn;
+    for(int btn = 0; btn < 4; ++btn){
+        snprintf(buf, 511, "%s %d %d", RelayCmd, R_BTNS, btn);
+        if(send_message(buf, &ans) && 8 == getRansArg(ans, canbuf) && canbuf[0] == R_BTNS){
+            r.buttons[btn] = canbuf[2];
+        }else goto rtn;
+    }
+    relay = r;
+    ret = TRUE;
+rtn:
+    FREE(ans);
+    return ret;
+}
+
 static void send_message_nocheck(const char *msg){
     if(!msg || sockfd < 0) return;
     size_t L = strlen(msg);
@@ -331,7 +402,9 @@ static int pusi_connect_server(){
     send_message_nocheck(registerUaxe);
     send_message_nocheck(registerVaxe);
     send_message_nocheck(registerFocus);
+    send_message_nocheck(registerRelay);
     int retval = TRUE;
+    if(!chkRelay()) retval = FALSE;
     if(!setSpeed(setUspeed, "U")) retval = FALSE;
     if(!setSpeed(setVspeed, "V")) retval = FALSE;
     if(!setSpeed(setFspeed, "F")) retval = FALSE;
@@ -401,6 +474,7 @@ static int move_motor(const char *movecmd, int s){
         WARNX("can't send message");
         LOGDBG("can't send message");
         pusi_disconnect();
+        FREE(ans);
         return FALSE;
     }
     int ret = TRUE;
@@ -710,8 +784,19 @@ static char *pusi_status(const char *messageid, char *buf, int buflen){
         for(int i = 0; i < 3; ++i){
             const char *stat = "stopping";
             if(*mv[i]) stat = "moving";
-            l = snprintf(bptr, buflen, "\"%s\": { \"status\": \"%s\", \"position\": %d }%s",
+            l = snprintf(bptr, buflen, "\"%s\": { \"status\": \"%s\", \"position\": %d }, ",
-                         motors[i], stat, *pos[i], (i==2)?"":", ");
+                         motors[i], stat, *pos[i]);
+            buflen -= l; bptr += l;
+        }
+        relaystate r = relay;
+        l = snprintf(bptr, buflen, "\"relay\": %d, ", r.relays);
+        buflen -= l; bptr += l;
+        for(int p = 0; p < 3; ++p){
+            l = snprintf(bptr, buflen, "\"PWM%d\": %d, ", p, r.PWM[p]);
+            buflen -= l; bptr += l;
+        }
+        for(int b = 0; b < 4; ++b){
+            l = snprintf(bptr, buflen, "\"button%d\": %d%s", b, r.buttons[b], (b==3)?"":", ");
             buflen -= l; bptr += l;
         }
     }
@@ -779,6 +864,8 @@ static void *pusi_process_states(_U_ void *arg){
             pusi_connect_server();
             continue;
         }
+        // check relay
+        chkRelay();
         if(moving_finished(Ustatus, &Uposition)) Umoving = FALSE;
         else Umoving = TRUE;
         if(moving_finished(Vstatus, &Vposition)) Vmoving = FALSE;
@@ -896,6 +983,36 @@ static char *Vmove(const char *val, char *buf, int buflen){
     snprintf(buf, buflen, OK);
     return buf;
 }
+static char *relaycmd(const char *val, char *buf, int buflen){
+    const char *ans = FAIL;
+    char *eq = NULL, *par = strdup(val);
+    char mbuf[512];
+    relaystate r = relay;
+    if((eq = strchr(par, '='))){
+        *eq++ = 0;
+        int v = atoi(eq), tmpno = 0;
+        if(1 == sscanf(par, "R%d", &tmpno)){ // relay command
+            if(tmpno == 1 || tmpno == 0){
+                int rval = r.relays;
+                if(v) rval |= 1<<tmpno;
+                else rval &= ~(1<<tmpno);
+                green("Relay %d -> %d", r.relays, rval);
+                snprintf(mbuf, 511, "%s %d %d", RelayCmd, R_RELAY + relaySetter, rval);
+                if(send_message(mbuf, NULL)) ans = OK;
+            }
+        }else if(1 == sscanf(par, "PWM%d", &tmpno)){ // PWM command
+            if(tmpno >= 0 && tmpno < 4 && v > -1 && v < 256){
+                green("PWM %d -> %d\n", tmpno, v);
+                r.PWM[tmpno] = v;
+                snprintf(mbuf, 511, "%s %d %u %u %u", RelayCmd, R_PWM + relaySetter, r.PWM[0], r.PWM[1], r.PWM[2]);
+                if(send_message(mbuf, NULL)) ans = OK;
+            }
+        }
+    }
+    FREE(par);
+    snprintf(buf, buflen, "%s", ans);
+    return buf;
+}
 
 steppersproc pusyCANbus = {
     .stepdisconnect = pusi_stop,
@@ -905,4 +1022,5 @@ steppersproc pusyCANbus = {
     .movefocus = set_pfocus,
     .moveByU = Umove,
     .moveByV = Vmove,
+    .relay = relaycmd,
 };

LocCorr/socket.c

@@ -76,11 +76,13 @@ static char *setstepperstate(const char *state, char *buf, int buflen);
 static char *setfocusstate(const char *state, char *buf, int buflen);
 static char *moveU(const char *val, char *buf, int buflen);
 static char *moveV(const char *val, char *buf, int buflen);
+static char *relaycmd(const char *val, char *buf, int buflen);
 static setter setterHandlers[] = {
     {"stpstate", setstepperstate, "Set given steppers' server state"},
     {"focus", setfocusstate, "Move focus to given value"},
     {"moveU", moveU, "Relative moving by U axe"},
     {"moveV", moveV, "Relative moving by V axe"},
+    {"relay", relaycmd, "Send relay commands (Rx=0/1, PWMX=0..255)"},
     {NULL, NULL, NULL}
 };
 
@@ -141,7 +143,10 @@ static char *moveV(const char *val, char *buf, int buflen){
     if(theSteppers && theSteppers->moveByV) return theSteppers->moveByU(val, buf, buflen);
     return retFAIL(buf, buflen);
 }
-
+static char *relaycmd(const char *val, char *buf, int buflen){
+    if(theSteppers && theSteppers->relay) return theSteppers->relay(val, buf, buflen);
+    return retFAIL(buf, buflen);
+}
 /*
 static char *rmnl(const char *msg, char *buf, int buflen){
     strncpy(buf, msg, buflen);
@@ -166,6 +171,7 @@ static char *processCommand(const char msg[BUFLEN], char *ans, int anslen){
         DBG("got KEY '%s' with value '%s'", kv, value);
         key_value result;
         par = chk_keyval(kv, value, &result);
+        FREE(kv);
         if(par){
             switch(par->type){
                 case PAR_INT:
@@ -191,7 +197,6 @@ static char *processCommand(const char msg[BUFLEN], char *ans, int anslen){
                 ++s;
             }
         }
-        FREE(kv);
     }else{
         getter *g = getterHandlers;
         while(g->command){