From 240b5f8677b0c570976f30b81ed96940bf7ca48e Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Wed, 23 Sep 2020 17:31:01 +0300 Subject: [PATCH] fixed some bugs --- commandline/Start_settings.cfg | 3 + commandline/canbus.c | 29 +++-- commandline/canbus.h | 1 + commandline/canopen.c | 11 +- commandline/cfg/Default_settings.cfg | 13 +++ commandline/cfg/Start_settings.cfg | 19 ++++ commandline/cfg/Start_settings_onlySpeed.cfg | 11 ++ commandline/cfg/endswitches.cfg | 13 +++ commandline/cfg/endswitchesON.cfg | 2 + commandline/cfg/move | 8 ++ commandline/cmdlnopts.c | 1 + commandline/cmdlnopts.h | 1 + commandline/dicentries.in | 45 ++++++++ commandline/main.c | 114 ++++++++++++------- 14 files changed, 222 insertions(+), 49 deletions(-) create mode 100644 commandline/cfg/Default_settings.cfg create mode 100644 commandline/cfg/Start_settings.cfg create mode 100644 commandline/cfg/Start_settings_onlySpeed.cfg create mode 100644 commandline/cfg/endswitches.cfg create mode 100644 commandline/cfg/endswitchesON.cfg create mode 100755 commandline/cfg/move diff --git a/commandline/Start_settings.cfg b/commandline/Start_settings.cfg index 5b0c2cd..da0ee65 100644 --- a/commandline/Start_settings.cfg +++ b/commandline/Start_settings.cfg @@ -1,6 +1,9 @@ # Transmit SDO to driver # Format: index, subindex, data +# Set heartbeat time to 0 +0x1017, 0, 0 + # Set Node ID to 1 0x2002, 0, 1 # Set speed to 250 (0-20, 1-25, 2-50, 3-100, 4-125, 5-250, 6-500, 7-800, 8-1000) diff --git a/commandline/canbus.c b/commandline/canbus.c index 694ee58..35dbab1 100644 --- a/commandline/canbus.c +++ b/commandline/canbus.c @@ -54,6 +54,7 @@ static char *read_string(); * @return amount of bytes read */ static int read_ttyX(TTY_descr *d){ + FNAME(); if(!d || d->comfd < 0) return -1; size_t L = 0; ssize_t l; @@ -85,7 +86,10 @@ static int read_ttyX(TTY_descr *d){ // thread-safe writing, add trailing '\n' static int ttyWR(const char *buff, int len){ + FNAME(); pthread_mutex_lock(&mutex); + //canbus_clear(); + read_string(); // clear RX buffer DBG("Write 2tty %d bytes: ", len); #ifdef EBUG int _U_ n = write(STDERR_FILENO, buff, len); @@ -95,10 +99,16 @@ static int ttyWR(const char *buff, int len){ int w = write_tty(dev->comfd, buff, (size_t)len); if(!w) w = write_tty(dev->comfd, "\n", 1); DBG("Written, dt=%g", dtime() - t0); - char *s = read_string(); // clear echo & check - if(!s || strcmp(s, buff) != 0){ - WARNX("wrong answer! Got '%s' instead of '%s'", s, buff); - return 1; + int errctr = 0; + while(1){ + char *s = read_string(); // clear echo & check + if(!s || strncmp(s, buff, strlen(buff)) != 0){ + if(++errctr > 3){ + WARNX("wrong answer! Got '%s' instead of '%s'", s, buff); + w = 1; + break; + } + }else break; } pthread_mutex_unlock(&mutex); DBG("Success, dt=%g", dtime() - t0); @@ -113,7 +123,7 @@ void setserialspeed(int speed){ serialspeed = speed; } -static void clearRXbuf(){ +void canbus_clear(){ while(read_ttyX(dev)); } @@ -143,11 +153,13 @@ int canbus_setspeed(int speed){ } int len = snprintf(buff, BUFLEN, "b %d", speed); if(len < 1) return 2; - clearRXbuf(); - return ttyWR(buff, len); + int r = ttyWR(buff, len); + read_string(); // clear RX buf ('Reinit CAN bus with speed XXXXkbps') + return r; } int canbus_write(CANmesg *mesg){ + FNAME(); char buf[BUFLEN]; if(!mesg || mesg->len > 8) return 1; int rem = BUFLEN, len = 0; @@ -158,7 +170,7 @@ int canbus_write(CANmesg *mesg){ rem -= l; len += l; if(rem < 0) return 2; } - clearRXbuf(); + canbus_clear(); return ttyWR(buf, len); } @@ -167,6 +179,7 @@ int canbus_write(CANmesg *mesg){ * @return NULL if nothing was read or pointer to static buffer */ static char *read_string(){ + FNAME(); static char buf[1024]; int LL = 1023, r = 0, l; char *ptr = NULL; diff --git a/commandline/canbus.h b/commandline/canbus.h index f390cdc..f2c2bc8 100644 --- a/commandline/canbus.h +++ b/commandline/canbus.h @@ -38,6 +38,7 @@ int canbus_open(const char *devname); int canbus_write(CANmesg *mesg); int canbus_read(CANmesg *mesg); int canbus_setspeed(int speed); +void canbus_clear(); // auxiliary (not necessary) functions void setserialspeed(int speed); diff --git a/commandline/canopen.c b/commandline/canopen.c index 52a14dc..17800d9 100644 --- a/commandline/canopen.c +++ b/commandline/canopen.c @@ -157,6 +157,7 @@ static int ask2read(uint16_t idx, uint8_t subidx, uint8_t NID){ } static SDO *getSDOans(uint16_t idx, uint8_t subidx, uint8_t NID){ + FNAME(); CANmesg mesg; SDO *sdo = NULL; double t0 = dtime(); @@ -184,8 +185,9 @@ static SDO *getSDOans(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){ + FNAME(); if(ask2read(idx, subidx, NID)){ - WARNX("Can't initiate upload"); + WARNX("readSDOvalue(): Can't initiate upload"); return NULL; } return getSDOans(idx, subidx, NID); @@ -217,16 +219,16 @@ static inline int8_t mki8(uint8_t data[4]){ // read SDO value, if error - return INT64_MIN int64_t SDO_read(const SDO_dic_entry *e, uint8_t NID){ + FNAME(); SDO *sdo = readSDOvalue(e->index, e->subindex, NID); if(!sdo){ - WARNX("SDO read error"); return INT64_MIN; } 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); - if(etxt) red("Abort code 0x%X: ", ac, etxt); + if(etxt) WARNX("Abort code 0x%X: %s", ac, etxt); return INT64_MIN; } if(sdo->datalen != e->datasize){ @@ -261,6 +263,7 @@ int64_t SDO_read(const SDO_dic_entry *e, uint8_t NID){ // write SDO data, return 0 if all OK int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){ + FNAME(); if(!e || !data || e->datasize < 1 || e->datasize > 4){ WARNX("SDO_write(): bad datalen"); return 1; @@ -273,10 +276,12 @@ int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){ sdo.index = e->index; sdo.subindex = e->subindex; CANmesg *mesgp = mkMesg(&sdo); + DBG("Canbus write.."); if(canbus_write(mesgp)){ WARNX("SDO_write(): Can't initiate download"); return 2; } + DBG("get answer"); SDO *sdop = getSDOans(e->index, e->subindex, NID); if(!sdop){ WARNX("SDO_write(): SDO read error"); diff --git a/commandline/cfg/Default_settings.cfg b/commandline/cfg/Default_settings.cfg new file mode 100644 index 0000000..e690e76 --- /dev/null +++ b/commandline/cfg/Default_settings.cfg @@ -0,0 +1,13 @@ +# Transmit SDO to driver +# Format: index, subindex, data + +# Set heartbeat time to 1000 +0x1017, 0, 1000 + +# Set Node ID to 5 +0x2002, 0, 5 +# Set speed to 250 (0-20, 1-25, 2-50, 3-100, 4-125, 5-250, 6-500, 7-800, 8-1000) +0x2003, 0, 4 + +# Save parameters +0x2007, 0, 2 diff --git a/commandline/cfg/Start_settings.cfg b/commandline/cfg/Start_settings.cfg new file mode 100644 index 0000000..8e9d77d --- /dev/null +++ b/commandline/cfg/Start_settings.cfg @@ -0,0 +1,19 @@ +# Transmit SDO to driver +# Format: index, subindex, data + +# Set heartbeat time to 0 +0x1017, 0, 0 + +# Set Node ID to 1 +0x2002, 0, 2 +# Set speed to 250 (0-20, 1-25, 2-50, 3-100, 4-125, 5-250, 6-500, 7-800, 8-1000) +0x2003, 0, 5 + +# EXT1&EXT2 emergency stop: EXT1@falling edge (normal state == 1), EXT2@rising edge (normal state == 0) +# Falling edge for trigger #1 +0x600F, 2, 0 +# Enable EXT1, EXT2 +0x600F, 1, 1 + +# Save parameters +0x2007, 0, 2 diff --git a/commandline/cfg/Start_settings_onlySpeed.cfg b/commandline/cfg/Start_settings_onlySpeed.cfg new file mode 100644 index 0000000..5fc5d47 --- /dev/null +++ b/commandline/cfg/Start_settings_onlySpeed.cfg @@ -0,0 +1,11 @@ +# Transmit SDO to driver +# Format: index, subindex, data + +# Set heartbeat time to 0 +0x1017, 0, 0 + +# Set speed to 250 (0-20, 1-25, 2-50, 3-100, 4-125, 5-250, 6-500, 7-800, 8-1000) +0x2003, 0, 5 + +# Save parameters +0x2007, 0, 2 diff --git a/commandline/cfg/endswitches.cfg b/commandline/cfg/endswitches.cfg new file mode 100644 index 0000000..0d88c6b --- /dev/null +++ b/commandline/cfg/endswitches.cfg @@ -0,0 +1,13 @@ +# Transmit SDO to driver +# Format: index, subindex, data + +# EXT1&EXT2 emergency stop: EXT1@falling edge (normal state == 1), EXT2@rising edge (normal state == 0) +# Falling edge for trigger #1 (inverted!), rising for trigger #2 +0x600F, 2, 3 +# Pullup for trigger #2 +0x600F, 3, 2 +# Enable EXT1, EXT2 +0x600F, 1, 3 + +# save settings +#0x2007, 0, 2 diff --git a/commandline/cfg/endswitchesON.cfg b/commandline/cfg/endswitchesON.cfg new file mode 100644 index 0000000..fff1c4f --- /dev/null +++ b/commandline/cfg/endswitchesON.cfg @@ -0,0 +1,2 @@ +# Enable EXT1, EXT2 +0x600F, 1, 3 diff --git a/commandline/cfg/move b/commandline/cfg/move new file mode 100755 index 0000000..f063d2a --- /dev/null +++ b/commandline/cfg/move @@ -0,0 +1,8 @@ +#!/bin/bash + +while true; do + ./steppermove -m100 -w -I4 -r 32 + ./steppermove -m200 -w -I4 -r -64 + sleep 1 +done + diff --git a/commandline/cmdlnopts.c b/commandline/cmdlnopts.c index 2648b45..77fc57d 100644 --- a/commandline/cmdlnopts.c +++ b/commandline/cmdlnopts.c @@ -77,6 +77,7 @@ static myoption cmdlnopts[] = { {"disable", NO_ARGS, NULL, 'D', arg_int, APTR(&G.disable), _("disable motor")}, {"readvals",NO_ARGS, NULL, 'R', arg_int, APTR(&G.showpars), _("read values of used parameters")}, {"enablesw",NO_ARGS, NULL, 'E', arg_int, APTR(&G.enableESW), _("enable end-switches 1 and 2")}, + {"wait", NO_ARGS, NULL, 'w', arg_int, APTR(&G.wait), _("wait while motor is busy")}, end_option }; diff --git a/commandline/cmdlnopts.h b/commandline/cmdlnopts.h index 9bbee87..2462688 100644 --- a/commandline/cmdlnopts.h +++ b/commandline/cmdlnopts.h @@ -50,6 +50,7 @@ typedef struct{ int disable; // disable motor int showpars; // show values of some parameters int enableESW; // send signal to enable end-switches + int wait; // wait while device is busy } glob_pars; diff --git a/commandline/dicentries.in b/commandline/dicentries.in index a058e76..1413d93 100644 --- a/commandline/dicentries.in +++ b/commandline/dicentries.in @@ -22,6 +22,51 @@ // heartbeat time DICENTRY(HEARTBTTIME, 0x1017, 0, 2, 0, "heartbeat time") + +// receive PDO parameter 0 +// largest subindex supported +DICENTRY(RPDOP0LS, 0x1400, 0, 1, 0, "receive PDO parameter 0, largest subindex supported") +// COB-ID used by PDO +DICENTRY(RPDOP0CI, 0x1400, 1, 4, 0, "receive PDO parameter 0, COB-ID used by PDO") +// transmission type +DICENTRY(RPDOP0TT, 0x1400, 2, 1, 0, "receive PDO parameter 0, transmission type") +// inhibit time +DICENTRY(RPDOP0IT, 0x1400, 3, 2, 0, "receive PDO parameter 0, inhibit time") +// compatibility entry +DICENTRY(RPDOP0CE, 0x1400, 4, 1, 0, "receive PDO parameter 0, compatibility entry") +// event timer +DICENTRY(RPDOP0ET, 0x1400, 5, 2, 0, "receive PDO parameter 0, event timer") + +// receive PDO mapping 0 +// number of mapped application objects +DICENTRY(RPDOM0N, 0x1600, 0, 1, 0, "receive PDO mapping 0, number of objects") +// first map +DICENTRY(RPDOM0O1, 0x1600, 1, 4, 0, "receive PDO mapping 0, mapping for 1st object") + +// transmit PDO parameter 0 +// largest subindex supported +DICENTRY(TPDOP0LS, 0x1800, 0, 1, 0, "transmit PDO parameter 0, largest subindex supported") +// COB-ID used by PDO +DICENTRY(TPDOP0CI, 0x1800, 1, 4, 0, "transmit PDO parameter 0, COB-ID used by PDO") +// transmission type +DICENTRY(TPDOP0TT, 0x1800, 2, 1, 0, "transmit PDO parameter 0, transmission type") +// inhibit time +DICENTRY(TPDOP0IT, 0x1800, 3, 2, 0, "transmit PDO parameter 0, inhibit time") +// reserved +DICENTRY(TPDOP0R, 0x1800, 4, 1, 0, "transmit PDO parameter 0, reserved") +// event timer +DICENTRY(TPDOP0ET, 0x1800, 5, 2, 0, "transmit PDO parameter 0, event timer") + +// transmit PDO mapping 0 +// number of mapped application objects +DICENTRY(TPDOM0N, 0x1A00, 0, 1, 0, "transmit PDO mapping 0, number of objects") +// first map +DICENTRY(TPDOM0O1, 0x1A00, 1, 4, 0, "transmit PDO mapping 0, mapping for 1st object") +DICENTRY(TPDOM0O2, 0x1A00, 2, 4, 0, "transmit PDO mapping 0, mapping for 2nd object") +DICENTRY(TPDOM0O3, 0x1A00, 3, 4, 0, "transmit PDO mapping 0, mapping for 3rd object") +DICENTRY(TPDOM0O4, 0x1A00, 4, 4, 0, "transmit PDO mapping 0, mapping for 4th object") +DICENTRY(TPDOM0O5, 0x1A00, 5, 4, 0, "transmit PDO mapping 0, mapping for 5th object") + // node ID DICENTRY(NODEID, 0x2002, 0, 1, 0, "node ID") // baudrate diff --git a/commandline/main.c b/commandline/main.c index 0f2a386..803345a 100644 --- a/commandline/main.c +++ b/commandline/main.c @@ -16,7 +16,6 @@ * along with this program. If not, see . */ -#include "dataparser.h" #include #include #include @@ -27,11 +26,13 @@ #include "canbus.h" #include "canopen.h" #include "cmdlnopts.h" +#include "dataparser.h" #include "pusirobot.h" static glob_pars *GP = NULL; // for GP->pidfile need in `signals` static uint8_t ID = 0; static uint16_t microstepping = 0; +static uint8_t devstat = 0; // device status after chkstat() // default signal handler void signals(int sig){ @@ -66,15 +67,15 @@ static inline void chkerr(int64_t es){ static inline void chkstat(int64_t es){ if(es) red("DEVSTATUS=%d\n", (int)es); else green("DEVSTATUS=0\n"); - uint8_t s = (uint8_t)es; - if(s){ + devstat = (uint8_t)es; + if(devstat){ for(uint8_t i = 0; i < 8; ++i){ - const char *msg = devstatus(s, i); + const char *msg = devstatus(devstat, i); if(msg) red("\t%s\n", msg); } - if(s != BUSY_STATE && GP->clearerr && (SDO_write(&DEVSTATUS, ID, s) || 0 != (es = SDO_read(&DEVSTATUS, ID)))){ + if(devstat != BUSY_STATE && GP->clearerr && (SDO_write(&DEVSTATUS, ID, devstat) || 0 != (es = SDO_read(&DEVSTATUS, ID)))){ ERRX("Can't clean device status"); - } + }else devstat = (uint8_t)es; if(es && es != BUSY_STATE) ERRX("Can't work in this state"); // DIE if !busy } } @@ -122,7 +123,7 @@ static inline void gpioval(int64_t es){ // show values of all parameters from dicentries.in static inline void showAllPars(){ - green("\nParameters' values:"); + green("\nParameters' values:\n"); for(int i = 0; i < DEsz; ++i){ const SDO_dic_entry *entry = allrecords[i]; int64_t val = SDO_read(entry, GP->NodeID); @@ -131,12 +132,37 @@ static inline void showAllPars(){ entry->index, entry->subindex, entry->name); continue; } - printf("\n# %s\n0x%04X, %d, %ld", entry->name, entry->index, + printf("# %s\n0x%04X, %d, %ld\n", entry->name, entry->index, entry->subindex, val); + fflush(stdout); } printf("\n\n"); } +// wait while device is in busy state +static inline void wait_busy(){ + int errctr = 0; + int64_t ans; + int i = 0; + const char symbols[] = "|/-\\"; + printf("Waiting... "); + do{ + printf("%c\b", symbols[i]); + fflush(stdout); + if(++i > 3) i = 0; + if(INT64_MIN == (ans = SDO_read(&DEVSTATUS, ID))){ + if(++errctr > 10) ERRX("Can't read device status"); + }else errctr = 0; + if(ans == BUSY_STATE){ + usleep(50000); + }else{ + chkstat(ans); + break; + } + }while(1); + printf("\n"); +} + int main(int argc, char *argv[]){ initial_setup(); char *self = strdup(argv[0]); @@ -151,6 +177,7 @@ int main(int argc, char *argv[]){ signal(SIGINT, signals); // ctrl+C - quit signal(SIGQUIT, signals); // ctrl+\ - quit signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z + if(GP->NodeID != 1){ if(GP->NodeID < 1 || GP->NodeID > 127) ERRX("Node ID should be a number from 1 to 127"); } @@ -175,6 +202,7 @@ int main(int argc, char *argv[]){ putlog("Can't set CAN speed %d. Exit.", GP->canspeed); signals(2); } + DBG("here"); //setup_con(); // print current position and state @@ -185,45 +213,39 @@ int main(int argc, char *argv[]){ #define Mesg(...) //#define Mesg(...) green(__VA_ARGS__) - //double d0 = dtime(); +//double d0 = dtime(); getSDOe(ERRSTATE, chkerr, "Can't get error status"); Mesg("ERRSTATE: %g\n", dtime() - d0); + + if(GP->stop){ + if(SDO_write(&STOP, ID, 1)) ERRX("Can't stop motor"); + Mesg("STOP: %g\n", dtime() - d0); + } + getSDOe(DEVSTATUS, chkstat, "Can't get device status"); Mesg("DEVSTATUS: %g\n", dtime() - d0); - if(GP->parsefile){ - green("Try to parse %s and send SDOs to device\n", GP->parsefile); - parse_data_file(GP->parsefile, GP->NodeID); - Mesg("parse_data_file: %g\n", dtime() - d0); + + if(GP->zeropos){ + if(SDO_write(&POSITION, ID, 0)) + ERRX("Can't clear position counter"); + Mesg("POSITION: %g\n", dtime() - d0); } + getSDOe(MICROSTEPS, setusteps, "Can't get microstepping"); + Mesg("MICROSTEPS: %g\n", dtime() - d0); + + if(INT64_MIN != (i64 = SDO_read(&POSITION, ID))) + green("CURPOS=%d\n", (int)i64/microstepping); + else WARNX("Can't read current position"); + Mesg("CURPOS: %g\n", dtime() - d0); + if(GP->showpars){ showAllPars(); Mesg("showAllPars: %g\n", dtime() - d0); } - if(GP->enableESW){ - if(SDO_write(&EXTENABLE, ID, 3)){ - WARNX("Error when trying to enable limit switches"); - if(GP->absmove || GP->relmove) signals(-1); - } - Mesg("EXTENABLE: %g\n", dtime() - d0); - } - - getSDOe(MICROSTEPS, setusteps, "Can't get microstepping"); - Mesg("MICROSTEPS: %g\n", dtime() - d0); - if(GP->zeropos){ - if(SDO_write(&POSITION, ID, 0)) - ERRX("Can't clear position counter"); - Mesg("POSITION: %g\n", dtime() - d0); - } - if(INT64_MIN != (i64 = SDO_read(&POSITION, ID))) - green("CURPOS=%d\n", (int)i64/microstepping); - else WARNX("Can't read current position"); - Mesg("CURPOS: %g\n", dtime() - d0); - getSDOe(MAXSPEED, setmaxspd, "Can't read max speed"); - Mesg("MAXSPEED: %g\n", dtime() - d0); - getSDOw(GPIOVAL, gpioval, "Can't read GPIO values"); + getSDOe(GPIOVAL, gpioval, "Can't read GPIO values"); Mesg("GPIOVAL: %g\n", dtime() - d0); if(GP->disable){ @@ -237,18 +259,34 @@ int main(int argc, char *argv[]){ Mesg("Status: %g\n", dtime() - d0); } - if(GP->stop){ - if(SDO_write(&STOP, ID, 1)) ERRX("Can't stop motor"); - Mesg("STOP: %g\n", dtime() - d0); + if(devstat == BUSY_STATE && GP->wait) wait_busy(); + + if(GP->parsefile){ + green("Try to parse %s and send SDOs to device\n", GP->parsefile); + parse_data_file(GP->parsefile, GP->NodeID); + Mesg("parse_data_file: %g\n", dtime() - d0); } + if(GP->enableESW){ + if(SDO_write(&EXTENABLE, ID, 3)){ + WARNX("Error when trying to enable limit switches"); + if(GP->absmove || GP->relmove) signals(-1); + } + Mesg("EXTENABLE: %g\n", dtime() - d0); + } + + getSDOe(MAXSPEED, setmaxspd, "Can't read max speed"); + Mesg("MAXSPEED: %g\n", dtime() - d0); + if(GP->absmove != INT_MIN){ + if(devstat == BUSY_STATE) ERRX("Can't move in BUSY state"); SDO_write(&ENABLE, ID, 1); if(SDO_write(&ABSSTEPS, ID, GP->absmove*microstepping)) ERRX("Can't move to absolute position %d", GP->absmove); Mesg("AbsMove: %g\n", dtime() - d0); } if(GP->relmove != INT_MIN && GP->relmove){ + if(devstat == BUSY_STATE) ERRX("Can't move in BUSY state"); uint8_t dir = 1; SDO_write(&ENABLE, ID, 1); if(GP->relmove < 0){ // negative direction