diff --git a/LocCorr/basler.c b/LocCorr/basler.c index b9817e0..aa7e7c0 100644 --- a/LocCorr/basler.c +++ b/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; } diff --git a/LocCorr/improc.c b/LocCorr/improc.c index 1b8de53..92268f2 100644 --- a/LocCorr/improc.c +++ b/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); diff --git a/LocCorr/improc.h b/LocCorr/improc.h index e9422dd..0ff2c81 100644 --- a/LocCorr/improc.h +++ b/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; diff --git a/LocCorr/pusirobo.c b/LocCorr/pusirobo.c index 58a6a67..2910876 100644 --- a/LocCorr/pusirobo.c +++ b/LocCorr/pusirobo.c @@ -17,6 +17,7 @@ */ #define _GNU_SOURCE #include +#include #include #include #include @@ -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", - motors[i], stat, *pos[i], (i==2)?"":", "); + l = snprintf(bptr, buflen, "\"%s\": { \"status\": \"%s\", \"position\": %d }, ", + 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< %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, }; diff --git a/LocCorr/socket.c b/LocCorr/socket.c index 5ace5e2..61a204b 100644 --- a/LocCorr/socket.c +++ b/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){