From 57f4b9c1be529e8ce7fbe61cac6d38e2f4c48750 Mon Sep 17 00:00:00 2001 From: eddyem Date: Tue, 5 May 2020 01:12:09 +0300 Subject: [PATCH] a first approach to CLI --- commandline/Readme.md | 19 ++++++ commandline/canbus.c | 3 +- commandline/canopen.c | 138 ++++++++++++++++++++++++++++++++++------ commandline/canopen.h | 5 +- commandline/cmdlnopts.c | 14 +++- commandline/cmdlnopts.h | 5 ++ commandline/main.c | 96 ++++++++++++++++++++++++++-- commandline/pusirobot.c | 38 ++++++++++- commandline/pusirobot.h | 11 +++- 9 files changed, 298 insertions(+), 31 deletions(-) diff --git a/commandline/Readme.md b/commandline/Readme.md index 2c4a9c7..9df3792 100644 --- a/commandline/Readme.md +++ b/commandline/Readme.md @@ -3,3 +3,22 @@ Command line motor management CAN controller: my CAN-USB sniffer + +Usage: steppermove [args] + + Where args are: + + -0, --zeropos set current position to zero + -P, --pidfile=arg pidfile (default: /tmp/steppersmng.pid) + -S, --stop stop motor + -a, --abs=arg move to absolute position (steps) + -c, --clearerr clear errors + -d, --device=arg serial device name (default: /dev/ttyUSB0) + -h, --help show this help + -i, --nodeid=arg node ID (1..127) + -l, --logfile=arg file to save logs + -m, --maxspd=arg maximal motor speed (steps per second) + -r, --rel=arg move to relative position (steps) + -s, --canspd=arg CAN bus speed (default: DEFAULT_SPEED) + -t, --serialspd=arg serial (tty) device speed (default: DEFAULT_SPEED) + -u, --microsteps=arg microstepping (0..256) diff --git a/commandline/canbus.c b/commandline/canbus.c index a96ae98..fe3f5df 100644 --- a/commandline/canbus.c +++ b/commandline/canbus.c @@ -100,6 +100,7 @@ static int ttyWR(const char *buff, int len){ return 1; } pthread_mutex_unlock(&mutex); + DBG("Success"); return w; } @@ -228,6 +229,7 @@ void showM(CANmesg *m){ } int canbus_read(CANmesg *mesg){ + FNAME(); if(!mesg) return 1; pthread_mutex_lock(&mutex); double t0 = dtime(); @@ -236,7 +238,6 @@ int canbus_read(CANmesg *mesg){ CANmesg *m; while(dtime() - t0 < T_POLLING_TMOUT){ // read answer if((ans = read_string())){ // parse new data - pthread_mutex_unlock(&mutex); if((m = parseCANmesg(ans))){ DBG("Got canbus message:"); showM(m); diff --git a/commandline/canopen.c b/commandline/canopen.c index e2d9eff..dbe5222 100644 --- a/commandline/canopen.c +++ b/commandline/canopen.c @@ -75,9 +75,10 @@ const char *abortcode_text(uint32_t abortcode){ //, int *n){ do{ ++iter; uint32_t c = AC[idx].code; - printf("idx=%d, min=%d, max=%d\n", idx, min_, max_); + //printf("idx=%d, min=%d, max=%d\n", idx, min_, max_); if(c == abortcode){ //if(n) *n = iter; + //DBG("got : %s", AC[idx].errmsg); return AC[idx].errmsg; }else if(c > abortcode){ newidx = (idx + min_)/2; @@ -155,6 +156,26 @@ static int ask2read(uint16_t idx, uint8_t subidx, uint8_t NID){ return canbus_write(mesg); } +static SDO *getSDOans(uint16_t idx, uint8_t subidx, uint8_t NID){ + CANmesg mesg; + SDO *sdo = NULL; + double t0 = dtime(); + while(dtime() - t0 < SDO_ANS_TIMEOUT){ + mesg.ID = TSDO_COBID | NID; // read only from given ID + if(canbus_read(&mesg)){ + continue; + } + sdo = parseSDO(&mesg); + if(!sdo) continue; + if(sdo->index == idx && sdo->subindex == subidx) break; + } + if(!sdo || sdo->index != idx || sdo->subindex != subidx){ + WARNX("No answer from SDO 0x%X/0x%X", idx, subidx); + return NULL; + } + return sdo; +} + /** * @brief readSDOvalue - send request to SDO read * @param idx - SDO index @@ -163,25 +184,11 @@ static int ask2read(uint16_t idx, uint8_t subidx, uint8_t NID){ * @return SDO received or NULL if error */ SDO *readSDOvalue(uint16_t idx, uint8_t subidx, uint8_t NID){ - SDO *sdo = NULL; if(ask2read(idx, subidx, NID)){ WARNX("Can't initiate upload"); return NULL; } - CANmesg mesg; - double t0 = dtime(); - while(dtime() - t0 < SDO_ANS_TIMEOUT){ - mesg.ID = TSDO_COBID | NID; // read only from given ID - if(canbus_read(&mesg)) continue; - sdo = parseSDO(&mesg); - if(!sdo) continue; - if(sdo->index == idx && sdo->subindex == subidx) break; - } - if(!sdo || sdo->index != idx || sdo->subindex != subidx){ - WARNX("No answer for SDO reading"); - return NULL; - } - return sdo; + return getSDOans(idx, subidx, NID); } static inline uint32_t mku32(uint8_t data[4]){ @@ -215,7 +222,7 @@ int64_t SDO_read(const SDO_dic_entry *e, uint8_t NID){ WARNX("SDO read error"); return INT64_MIN; } - if(sdo->datalen == 0){ + if(sdo->ccs == CCS_ABORT_TRANSFER){ // error WARNX("Got error for SDO 0x%X", e->index); uint32_t ac = mku32(sdo->data); const char *etxt = abortcode_text(ac); @@ -252,8 +259,101 @@ int64_t SDO_read(const SDO_dic_entry *e, uint8_t NID){ return ans; } +// write SDO data +int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, uint8_t *data){ + if(!e || !data || e->datasize < 1 || e->datasize > 4){ + WARNX("SDO_write(): bad datalen"); + return 1; + } + SDO sdo; + sdo.NID = NID; + sdo.ccs = CCS_INIT_DOWNLOAD; + sdo.datalen = e->datasize; + for(uint8_t i = 0; i < e->datasize; ++i) sdo.data[i] = data[i]; + sdo.index = e->index; + sdo.subindex = e->subindex; + CANmesg *mesgp = mkMesg(&sdo); + if(canbus_write(mesgp)){ + WARNX("SDO_write(): Can't initiate download"); + return 2; + } + SDO *sdop = getSDOans(e->index, e->subindex, NID); + if(!sdop){ + WARNX("SDO_write(): SDO read error"); + return 3; + } + if(sdop->ccs == CCS_ABORT_TRANSFER){ // error + WARNX("SDO_write(): Got error for SDO 0x%X", e->index); + uint32_t ac = mku32(sdop->data); + const char *etxt = abortcode_text(ac); + if(etxt) WARNX("Abort code 0x%X: %s", ac, etxt); + return 4; + } + if(sdop->datalen != 0){ + WARNX("SDO_write(): got answer with non-zero length"); + return 5; + } + if(sdop->ccs != CCS_SEG_UPLOAD){ + WARNX("SDO_write(): got wrong answer"); + return 6; + } + return 0; +} + +int SDO_write(const SDO_dic_entry *e, _U_ uint8_t NID, int64_t data){ + if(!e) return 1; + uint8_t arr[4] = {0}; + uint32_t U; + int32_t I; + uint16_t U16; + int16_t I16; + if(e->issigned){ + switch(e->datasize){ + case 1: + arr[0] = (uint8_t) data; + break; + case 4: + I = (int32_t) data; + arr[0] = I&0xff; + arr[1] = (I>>8)&0xff; + arr[2] = (I>>16)&0xff; + arr[3] = (I>>24)&0xff; + break; + default: // can't be 3! 3->2 + I16 = (int16_t) data; + arr[0] = I16&0xff; + arr[1] = (I16>>8)&0xff; + } + }else{ + switch(e->datasize){ + case 1: + arr[0] = (uint8_t) data; + break; + case 4: + U = (uint32_t) data; + arr[0] = U&0xff; + arr[1] = (U>>8)&0xff; + arr[2] = (U>>16)&0xff; + arr[3] = (U>>24)&0xff; + break; + default: // can't be 3! 3->2 + U16 = (uint16_t) data; + arr[0] = U16&0xff; + arr[1] = (U16>>8)&0xff; + } + } + /* + DBG("DATA:"); + for(int i = 0; i < e->datasize; ++i) printf("0x%X ", arr[i]); + printf("\n"); + return 0;*/ + return SDO_writeArr(e, NID, arr); +} + + + // read one byte of data -int SDO_readByte(uint16_t idx, uint8_t subidx, uint8_t *data, uint8_t NID){ +/*int SDO_readByte(uint16_t idx, uint8_t subidx, uint8_t *data, uint8_t NID){ SDO *sdo = readSDOvalue(idx, subidx, NID); if(!sdo || sdo->datalen != 1){ WARNX("Got SDO with wrong data length: %d instead of 1", sdo->datalen); @@ -261,7 +361,7 @@ int SDO_readByte(uint16_t idx, uint8_t subidx, uint8_t *data, uint8_t NID){ } if(data) *data = sdo->data[0]; return 0; -} +}*/ #if 0 // write uint8_t to SDO with index idx & subindex subidx to NID diff --git a/commandline/canopen.h b/commandline/canopen.h index df4e8fc..904d717 100644 --- a/commandline/canopen.h +++ b/commandline/canopen.h @@ -81,7 +81,10 @@ const char *abortcode_text(uint32_t abortcode); SDO *parseSDO(CANmesg *mesg); SDO *readSDOvalue(uint16_t idx, uint8_t subidx, uint8_t NID); -int SDO_readByte(uint16_t idx, uint8_t subidx, uint8_t *data, uint8_t NID); int64_t SDO_read(const SDO_dic_entry *e, uint8_t NID); +int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, uint8_t *data); +int SDO_write(const SDO_dic_entry *e, uint8_t NID, int64_t data); + +//int SDO_readByte(uint16_t idx, uint8_t subidx, uint8_t *data, uint8_t NID); #endif // CANOPEN_H__ diff --git a/commandline/cmdlnopts.c b/commandline/cmdlnopts.c index cb4c2af..32c0179 100644 --- a/commandline/cmdlnopts.c +++ b/commandline/cmdlnopts.c @@ -25,6 +25,7 @@ #include #include "cmdlnopts.h" +#include "pusirobot.h" /* * here are global parameters initialisation @@ -46,7 +47,9 @@ static glob_pars const Gdefault = { .logfile = NULL, // don't save logs .NodeID = 1, // default node ID = 1 .absmove = INT_MIN, - .relmove = INT_MIN + .relmove = INT_MIN, + .microsteps = -1, + .maxspeed = INT_MIN }; /* @@ -62,8 +65,13 @@ static myoption cmdlnopts[] = { {"logfile", NEED_ARG, NULL, 'l', arg_string, APTR(&G.logfile), _("file to save logs")}, {"pidfile", NEED_ARG, NULL, 'P', arg_string, APTR(&G.pidfile), _("pidfile (default: " DEFAULT_PIDFILE ")")}, {"nodeid", NEED_ARG, NULL, 'i', arg_int, APTR(&G.NodeID), _("node ID (1..127)")}, - {"rel", NEED_ARG, NULL, 'r', arg_int, APTR(&G.relmove), _("move to relative position")}, - {"abs", NEED_ARG, NULL, 'r', arg_int, APTR(&G.absmove), _("move to absolute position")}, + {"microsteps", NEED_ARG,NULL, 'u', arg_int, APTR(&G.microsteps),_("microstepping (0..256)")}, + {"rel", NEED_ARG, NULL, 'r', arg_int, APTR(&G.relmove), _("move to relative position (steps)")}, + {"abs", NEED_ARG, NULL, 'a', arg_int, APTR(&G.absmove), _("move to absolute position (steps)")}, + {"maxspd", NEED_ARG, NULL, 'm', arg_int, APTR(&G.maxspeed), _("maximal motor speed (steps per second)")}, + {"stop", NO_ARGS, NULL, 'S', arg_int, APTR(&G.stop), _("stop motor")}, + {"clearerr",NO_ARGS, NULL, 'c', arg_int, APTR(&G.clearerr), _("clear errors")}, + {"zeropos", NO_ARGS, NULL, '0', arg_int, APTR(&G.zeropos), _("set current position to zero")}, end_option }; diff --git a/commandline/cmdlnopts.h b/commandline/cmdlnopts.h index e33a516..80d4d86 100644 --- a/commandline/cmdlnopts.h +++ b/commandline/cmdlnopts.h @@ -40,6 +40,11 @@ typedef struct{ int NodeID; // node ID to work with int absmove; // absolute position to move int relmove; // relative position to move + int microsteps; // set microstepping + int maxspeed; // max speed + int stop; // stop motor + int clearerr; // try to clear errors + int zeropos; // set position to zero } glob_pars; diff --git a/commandline/main.c b/commandline/main.c index c9084e3..d8e054b 100644 --- a/commandline/main.c +++ b/commandline/main.c @@ -32,6 +32,7 @@ static glob_pars *GP = NULL; // for GP->pidfile need in `signals` void signals(int sig){ putlog("Exit with status %d", sig); + DBG("Exit with status %d", sig); restore_console(); if(GP->pidfile) // remove unnesessary PID file unlink(GP->pidfile); @@ -57,6 +58,15 @@ int main(int argc, char *argv[]){ if(GP->NodeID != 1){ if(GP->NodeID < 1 || GP->NodeID > 127) ERRX("Node ID should be a number from 1 to 127"); } + if(GP->microsteps > 0 && (1 != __builtin_popcount(GP->microsteps) || GP->microsteps == 1)) // __builtin_popcount - amount of non-zero bits in uint + ERRX("Wrong microstepping settings, should be 0 or 2^(1..8)"); + if(GP->absmove != INT_MIN || GP->relmove != INT_MIN){ // wanna move + if(GP->absmove != INT_MIN && GP->relmove != INT_MIN) + ERRX("ABSMOVE and RELMOVE can't be used together"); + if(GP->maxspeed == 0) + ERRX("Set non-zero MAXSPEED"); + } + if(GP->logfile) openlogfile(GP->logfile); putlog(("Start application...")); putlog("Try to open CAN bus device %s", GP->device); @@ -73,13 +83,89 @@ int main(int argc, char *argv[]){ //setup_con(); // print current position and state int64_t i64; - if(INT64_MIN != (i64 = SDO_read(&DEVSTATUS, GP->NodeID))) + uint8_t ID = GP->NodeID; + if(INT64_MIN != (i64 = SDO_read(&ERRSTATE, ID))){ + if(i64){ + red("ERRSTATE=%d\n", i64); + uint8_t s = (uint8_t)i64; + for(uint8_t i = 0; i < 8; ++i){ + const char *msg = errname(s, i); + if(msg) red("\t%s\n", msg); + } + if(!GP->clearerr) ERRX("Error status is not zero"); + if(SDO_write(&ERRSTATE, ID, s) || 0 != (i64 = SDO_read(&ERRSTATE, ID))){ + ERRX("Can't clean error status"); + } + } + } + if(INT64_MIN != (i64 = SDO_read(&DEVSTATUS, ID))){ green("DEVSTATUS=%d\n", (int)i64); - if(INT64_MIN != (i64 = SDO_read(&POSITION, GP->NodeID))) - green("CURPOS=%d\n", (int)i64); - if(INT64_MIN != (i64 = SDO_read(&MAXSPEED, GP->NodeID))) - green("MAXSPEED=%d\n", (int)i64); + uint8_t s = (uint8_t)i64; + if(s){ + for(uint8_t i = 0; i < 8; ++i){ + const char *msg = devstatus(s, i); + if(msg) red("\t%s\n", msg); + } + if(s != BUSY_STATE && GP->clearerr && (SDO_write(&DEVSTATUS, ID, s) || 0 != (i64 = SDO_read(&DEVSTATUS, ID)))){ + ERRX("Can't clean device status"); + } + if(i64 && i64 != BUSY_STATE) ERRX("Can't work in this state"); // DIE if !busy + } + }else ERRX("Can't get device status"); + if(GP->zeropos){ + i64 = 0; + if(SDO_write(&POSITION, ID, i64)) + ERRX("Can't clear position counter"); + } + uint16_t microstepping = 0; + if(INT64_MIN != (i64 = SDO_read(&MICROSTEPS, ID))){ + if(GP->microsteps > -1 && GP->microsteps != (int) i64){ + DBG("Try to change microsteps"); + if(SDO_write(&MICROSTEPS, ID, GP->microsteps) || INT64_MIN == (i64 = SDO_read(&MICROSTEPS, ID))) + ERRX("Can't change microstepping"); + } + microstepping = (uint16_t) i64; + green("MICROSTEPPING=%u\n", microstepping); + }else ERRX("Can't get microstepping"); + if(INT64_MIN != (i64 = SDO_read(&POSITION, ID))) + green("CURPOS=%d\n", (int)i64/microstepping); + else ERRX("Can't read current position"); + if(INT64_MIN != (i64 = SDO_read(&MAXSPEED, ID))){ + DBG("abs=%d, rel=%d", GP->absmove, GP->relmove); + if(i64 == 0 && (GP->absmove != INT_MIN || GP->relmove != INT_MIN) && (GP->maxspeed == INT_MIN || GP->maxspeed == 0)) + ERRX("Can't move when MAXSPEED==0"); + if(GP->maxspeed != INT_MIN){ + GP->maxspeed *= microstepping; + if(GP->maxspeed < MAX_SPEED_MIN || GP->maxspeed > MAX_SPEED_MAX) + ERRX("MAXSPEED should be from %d to %d", MAX_SPEED_MIN/microstepping, MAX_SPEED_MAX/microstepping); + DBG("Try to change max speed"); + if(SDO_write(&MAXSPEED, ID, GP->maxspeed) || INT64_MIN == (i64 = SDO_read(&MAXSPEED, ID))) + ERRX("Can't change max speed"); + } + green("MAXSPEED=%d\n", (int)i64/microstepping); + }else ERRX("Can't read max speed"); + if(GP->stop){ + if(SDO_write(&STOP, ID, 1)) + ERRX("Can't stop motor"); + } + + if(GP->absmove != INT_MIN){ + if(SDO_write(&ABSSTEPS, ID, GP->absmove*microstepping)) + ERRX("Can't move to absolute position %d", GP->absmove); + } + if(GP->relmove != INT_MIN && GP->relmove){ + uint8_t dir = 1; + if(GP->relmove < 0){ // negative direction + dir = 0; + GP->relmove = -GP->relmove; + } + if(SDO_write(&ROTDIR, ID, dir) || INT64_MIN == (i64 = SDO_read(&ROTDIR, ID))) + ERRX("Can't change rotation direction"); + DBG("i64=%ld, dir=%d", i64, dir); + if(SDO_write(&RELSTEPS, ID, GP->relmove*microstepping)) + ERRX("Can't move to relative position %d", GP->relmove); + } #if 0 CANmesg m; double t = dtime() - 10.; diff --git a/commandline/pusirobot.c b/commandline/pusirobot.c index acbf556..4910a93 100644 --- a/commandline/pusirobot.c +++ b/commandline/pusirobot.c @@ -16,12 +16,48 @@ * along with this program. If not, see . */ -//#include +#include // for NULL // we should init constants here! #define DICENTRY(name, idx, sidx, sz, s) const SDO_dic_entry name = {idx, sidx, sz, s}; #include "pusirobot.h" +// controller status for bits +static const char *DevStatus[] = { + "External stop 1", + "External stop 2", + "Stall state", + "Busy state", + "External stop 3", + "The FIFO of PVT Mode 3 is empty", + "FIFO Lower bound of PVT Mode 3", + "FIFO upper limit of PVT mode 3" +}; + +// controller error statuses +static const char *DevError[] = { + "TSD, over temperature shutdown", + "AERR, coil A error", + "BERR, coil B error", + "AOC, A over current", + "BOC, B over current", + "UVLO, low voltage fault" +}; + +// return status message for given bit in status +const char *devstatus(uint8_t status, uint8_t bit){ + if(bit > 7) return NULL; + if(status & (1< 5) return NULL; + if(error & (1<