diff --git a/commandline/Start_settings.cfg b/commandline/Start_settings.cfg index 0a623c2..5b0c2cd 100644 --- a/commandline/Start_settings.cfg +++ b/commandline/Start_settings.cfg @@ -3,6 +3,8 @@ # 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) +0x2003, 0, 5 # Set microstepping to 32 0x600A, 0, 32 # Set max phase current to 600mA @@ -12,13 +14,13 @@ # Set deceleration 0x6009, 0, 5 -# EXT1 emergency stop -# Rising edge trigger -0x600F, 2, 1 -# Turn on pullup -0x600F, 3, 1 -# Enable EXT1 -0x600F, 1, 1 +# EXT1&EXT2 emergency stop: EXT1@falling edge (normal state == 1), EXT2@rising edge (normal state == 0) +# Rising edge trigger for #2, falling for #1 (inverted!) +0x600F, 2, 3 +# Turn on pullup for #2 (EXT1 is always pulled down) +0x600F, 3, 2 +# Enable EXT1, EXT2 +0x600F, 1, 3 # Save parameters 0x2007, 0, 2 diff --git a/commandline/canbus.c b/commandline/canbus.c index 6924125..3ec02fb 100644 --- a/commandline/canbus.c +++ b/commandline/canbus.c @@ -112,6 +112,11 @@ void setserialspeed(int speed){ serialspeed = speed; } +static void clearRXbuf(){ + double t0 = dtime(); + while(read_string() && dtime() - t0 < T_POLLING_TMOUT){usleep(1000);} +} + int canbus_open(const char *devname){ if(!devname){ WARNX("canbus_open(): need device name"); @@ -138,7 +143,7 @@ int canbus_setspeed(int speed){ } int len = snprintf(buff, BUFLEN, "b %d", speed); if(len < 1) return 2; - while(read_string()); // clear buffer + clearRXbuf(); return ttyWR(buff, len); } @@ -153,7 +158,7 @@ int canbus_write(CANmesg *mesg){ rem -= l; len += l; if(rem < 0) return 2; } - while(read_string()); // clear buffer + clearRXbuf(); return ttyWR(buf, len); } @@ -220,6 +225,7 @@ CANmesg *parseCANmesg(const char *str){ return &m; } +#if 0 void showM(CANmesg *m){ printf("TS=%d, ID=0x%X", m->timemark, m->ID); int l = m->len; @@ -227,6 +233,7 @@ void showM(CANmesg *m){ for(int i = 0; i < l; ++i) printf(" 0x%02X", m->data[i]); printf("\n"); } +#endif int canbus_read(CANmesg *mesg){ FNAME(); @@ -239,8 +246,8 @@ int canbus_read(CANmesg *mesg){ while(dtime() - t0 < T_POLLING_TMOUT){ // read answer if((ans = read_string())){ // parse new data if((m = parseCANmesg(ans))){ - DBG("Got canbus message:"); - showM(m); + //DBG("Got canbus message:"); + //showM(m); if(ID && m->ID == ID){ memcpy(mesg, m, sizeof(CANmesg)); DBG("All OK"); diff --git a/commandline/canopen.c b/commandline/canopen.c index dbe5222..32ed2e3 100644 --- a/commandline/canopen.c +++ b/commandline/canopen.c @@ -110,7 +110,7 @@ static CANmesg *mkMesg(SDO *sdo){ mesg.data[1] = sdo->index & 0xff; // l mesg.data[2] = (sdo->index >> 8) & 0xff; // h mesg.data[3] = sdo->subindex; -#ifdef EBUG +#if 0 FNAME(); green("Make message to 0x%X: ", mesg.ID); for(uint8_t i = 0; i < 8; ++i) printf("0x%02X ", mesg.data[i]); diff --git a/commandline/cmdlnopts.c b/commandline/cmdlnopts.c index e033b0f..2d1bb0f 100644 --- a/commandline/cmdlnopts.c +++ b/commandline/cmdlnopts.c @@ -74,6 +74,7 @@ static myoption cmdlnopts[] = { {"zeropos", NO_ARGS, NULL, '0', arg_int, APTR(&G.zeropos), _("set current position to zero")}, {"parse", NEED_ARG, NULL, 'p', arg_string, APTR(&G.parsefile), _("file with SDO data to send to device")}, {"check", NEED_ARG, NULL, 'k', arg_string, APTR(&G.checkfile), _("check SDO data file")}, + {"disable", NO_ARGS, NULL, 'D', arg_int, APTR(&G.disable), _("disable motor")}, end_option }; diff --git a/commandline/cmdlnopts.h b/commandline/cmdlnopts.h index 2ac586d..1ed3584 100644 --- a/commandline/cmdlnopts.h +++ b/commandline/cmdlnopts.h @@ -36,6 +36,7 @@ typedef struct{ char *pidfile; // name of PID file char *logfile; // logging to this file char *parsefile; // file to parse + char *checkfile; // SDO data filename to check int canspeed; // CAN bus speed int serialspeed; // serial device speed (CAN-bus to USB) int NodeID; // node ID to work with @@ -46,7 +47,7 @@ typedef struct{ int stop; // stop motor int clearerr; // try to clear errors int zeropos; // set position to zero - char *checkfile; // SDO data filename to check + int disable; // disable motor } glob_pars; diff --git a/commandline/dataparser.c b/commandline/dataparser.c index 941aaa2..9610771 100644 --- a/commandline/dataparser.c +++ b/commandline/dataparser.c @@ -125,7 +125,10 @@ int parse_data_file(const char *fname, uint8_t nid){ continue; } } - if(nid) SDO_write(entry, nid, data); + if(nid){ + if(SDO_write(entry, nid, data)) WARNX("Can't write SDO idx=0x%04X", entry->index); + else printf("Send to NID=%d SDO [0x%04X, 0x%02X] with data 0x%lX\n", nid, entry->index, entry->subindex, data); + } }while(0); if(!isgood){ WARNX("Bad syntax in line #%d: %s\nFormat: index, subindex, data (all may be hex, dec, oct or bin)", lineno, str); diff --git a/commandline/dicentries.h b/commandline/dicentries.h index 45d569e..06bf07f 100644 --- a/commandline/dicentries.h +++ b/commandline/dicentries.h @@ -60,6 +60,8 @@ DICENTRY(EXTENABLE, 0x600F, 1, 1, 0) DICENTRY(EXTTRIGMODE, 0x600F, 2, 1, 0) // EXT emergency sensor type DICENTRY(EXTSENSTYPE, 0x600F, 3, 1, 0) +// GPIO value +DICENTRY(GPIOVAL, 0x6012, 0, 2, 0) // absolute displacement DICENTRY(ABSSTEPS, 0x601C, 0, 4, 1) // stop motor diff --git a/commandline/main.c b/commandline/main.c index 85bd484..f7b6e3e 100644 --- a/commandline/main.c +++ b/commandline/main.c @@ -30,6 +30,8 @@ #include "pusirobot.h" static glob_pars *GP = NULL; // for GP->pidfile need in `signals` +static uint8_t ID = 0; +static uint16_t microstepping = 0; void signals(int sig){ putlog("Exit with status %d", sig); @@ -45,6 +47,74 @@ void iffound_default(pid_t pid){ ERRX("Another copy of this process found, pid=%d. Exit.", pid); } +static inline void chkerr(int64_t es){ + if(!es) return; + red("ERRSTATE=%d\n", es); + uint8_t s = (uint8_t)es; + 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 != (es = SDO_read(&ERRSTATE, ID))){ + ERRX("Can't clean error status"); + } +} + +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){ + 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 != (es = SDO_read(&DEVSTATUS, ID)))){ + ERRX("Can't clean device status"); + } + if(es && es != BUSY_STATE) ERRX("Can't work in this state"); // DIE if !busy + } +} + +static inline void setusteps(int64_t es){ + if(GP->microsteps > -1 && GP->microsteps != (int) es){ + DBG("Try to change microsteps"); + if(SDO_write(&MICROSTEPS, ID, GP->microsteps) || INT64_MIN == (es = SDO_read(&MICROSTEPS, ID))) + ERRX("Can't change microstepping"); + } + microstepping = (uint16_t) es; + green("MICROSTEPPING=%u\n", microstepping); +} + +static inline void setmaxspd(int64_t es){ + DBG("abs=%d, rel=%d", GP->absmove, GP->relmove); + if(es == 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 == (es = SDO_read(&MAXSPEED, ID))) + ERRX("Can't change max speed"); + } + if(es) green("MAXSPEED=%d\n", (int)es/microstepping); + else red("MAXSPEED=0\n"); +} + +static inline void gpioval(int64_t es){ + uint16_t v = (uint16_t) es; + if(INT64_MIN == (es = SDO_read(&EXTENABLE, ID))){ + WARNX("Can't read limit switches configuration"); + return; + } + for(int i = 1; i < 4; ++i){ + if(0 == (es & 1<<(i-1))) continue; // endswitch disabled + if(EXTACTIVE(i, v)) red("LIM%d=1\n", i); + } +} + int main(int argc, char *argv[]){ initial_setup(); char *self = strdup(argv[0]); @@ -92,79 +162,43 @@ int main(int argc, char *argv[]){ //setup_con(); // print current position and state int64_t i64; - 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); - 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"); + ID = GP->NodeID; +#define getSDOe(SDO, fn, e) do{if(INT64_MIN != (i64 = SDO_read(&SDO, ID))) fn(i64); else ERRX(e);}while(0) +#define getSDOw(SDO, fn, e) do{if(INT64_MIN != (i64 = SDO_read(&SDO, ID))) fn(i64); else WARNX(e);}while(0) + getSDOe(ERRSTATE, chkerr, "Can't get error status"); + getSDOe(DEVSTATUS, chkstat, "Can't get device status"); + getSDOe(MICROSTEPS, setusteps, "Can't get microstepping"); if(GP->zeropos){ - i64 = 0; - if(SDO_write(&POSITION, ID, i64)) + if(SDO_write(&POSITION, ID, 0)) 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"); + getSDOe(MAXSPEED, setmaxspd, "Can't read max speed"); + getSDOw(GPIOVAL, gpioval, "Can't read GPIO values"); + + if(GP->disable){ + if(SDO_write(&ENABLE, ID, 0)) ERRX("Can't disable motor"); + } + + if(INT64_MIN != (i64 = SDO_read(&ENABLE, ID))){ + if(i64) green("ENABLE=1\n"); + else red("ENABLE=0\n"); + } if(GP->stop){ - if(SDO_write(&STOP, ID, 1)) - ERRX("Can't stop motor"); + if(SDO_write(&STOP, ID, 1)) ERRX("Can't stop motor"); } if(GP->absmove != INT_MIN){ + SDO_write(&ENABLE, ID, 1); 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; + SDO_write(&ENABLE, ID, 1); if(GP->relmove < 0){ // negative direction dir = 0; GP->relmove = -GP->relmove; @@ -175,6 +209,8 @@ int main(int argc, char *argv[]){ if(SDO_write(&RELSTEPS, ID, GP->relmove*microstepping)) ERRX("Can't move to relative position %d", GP->relmove); } +#undef getSDOe + #if 0 CANmesg m; double t = dtime() - 10.; diff --git a/commandline/pusirobot.h b/commandline/pusirobot.h index 70e93ed..07f6473 100644 --- a/commandline/pusirobot.h +++ b/commandline/pusirobot.h @@ -38,6 +38,11 @@ typedef struct{ #define MAX_SPEED_MIN -200000 #define MAX_SPEED_MAX 200000 +// limit switches mask in GPIO status register (x=1,2,3) +#define EXTMASK(x) (1<<(6+x)) + +#define EXTACTIVE(x, reg) ((reg&EXTMASK(x)) ? 1:0) + // unclearable status #define BUSY_STATE (1<<3) const char *devstatus(uint8_t status, uint8_t bit);