From 1453398f088b6f3969b75b1e2030cbbb54b961bc Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Tue, 30 Aug 2022 16:50:17 +0300 Subject: [PATCH] fixed some little problems in `commandline` --- canserver/socket.c | 4 +- commandline/CMakeLists.txt | 2 +- commandline/OfflinePrgmng.cfg | 6 ++ commandline/Readme.md | 46 ++++++----- commandline/canopen.c | 12 ++- commandline/canopen.h | 4 +- commandline/cfg/Acceleration.cfg | 10 +++ commandline/cfg/NoPDOmapping.cfg | 2 + commandline/cfg/SetCurnt0.6.cfg | 8 ++ commandline/cfg/Start_settings.cfg | 15 +++- commandline/cfg/Start_settingsEXT1.cfg | 27 +++++++ commandline/cmdlnopts.c | 16 ++-- commandline/cmdlnopts.h | 10 +-- commandline/dataparser.c | 13 ++-- commandline/dicentries.in | 2 +- commandline/main.c | 101 +++++++++++++++---------- commandline/pusirobot.h | 3 + commandline/verblog.c | 38 ++++++++++ commandline/verblog.h | 32 ++++++++ 19 files changed, 258 insertions(+), 93 deletions(-) create mode 100644 commandline/OfflinePrgmng.cfg create mode 100644 commandline/cfg/Acceleration.cfg create mode 100644 commandline/cfg/NoPDOmapping.cfg create mode 100644 commandline/cfg/SetCurnt0.6.cfg create mode 100644 commandline/cfg/Start_settingsEXT1.cfg create mode 100644 commandline/verblog.c create mode 100644 commandline/verblog.h diff --git a/canserver/socket.c b/canserver/socket.c index 9973b63..252b8b6 100644 --- a/canserver/socket.c +++ b/canserver/socket.c @@ -53,12 +53,12 @@ message ServerMessages = {0}; */ static size_t send_data(int sock, const char *textbuf){ ssize_t Len = strlen(textbuf); - if(Len != write(sock, textbuf, Len)){ + if(Len != send(sock, textbuf, Len, MSG_NOSIGNAL)){ WARN("write()"); LOGERR("send_data(): write() failed"); return 0; }else LOGDBG("send_data(): sent '%s'", textbuf); - if(textbuf[Len-1] != '\n') Len += write(sock, "\n", 1); + if(textbuf[Len-1] != '\n') Len += send(sock, "\n", 1, MSG_NOSIGNAL); return (size_t)Len; } diff --git a/commandline/CMakeLists.txt b/commandline/CMakeLists.txt index c356a80..5cd0e2d 100644 --- a/commandline/CMakeLists.txt +++ b/commandline/CMakeLists.txt @@ -20,7 +20,7 @@ set(CMAKE_COLOR_MAKEFILE ON) # here is one of two variants: all .c in directory or .c files in list aux_source_directory(${CMAKE_CURRENT_SOURCE_DIR} SOURCES) -# cmake -DDEBUG=1 -> debugging +# cmake -DEBUG=1 -> debugging if(DEFINED EBUG) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wextra -Wall -Werror -W") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wextra -Wall -Werror -W") diff --git a/commandline/OfflinePrgmng.cfg b/commandline/OfflinePrgmng.cfg new file mode 100644 index 0000000..1e20ae2 --- /dev/null +++ b/commandline/OfflinePrgmng.cfg @@ -0,0 +1,6 @@ +# Turn off offline programming +0x6018, 1, 0 +0x6018, 2, 0 +# Save parameters +0x2007, 0, 2 + diff --git a/commandline/Readme.md b/commandline/Readme.md index c34cabf..42b0a5e 100644 --- a/commandline/Readme.md +++ b/commandline/Readme.md @@ -8,29 +8,33 @@ Usage: steppermove [args] Where args are: - -0, --zeropos set current position to zero - -D, --disable disable motor - -E, --enablesw enable end-switches 1 and 2 - -I, --nodeid=arg node ID (1..127) - -P, --pidfile=arg pidfile (default: /tmp/steppersmng.pid) - -R, --readvals read values of used parameters - -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 - -k, --check=arg check SDO data file - -l, --logfile=arg file to save logs - -m, --maxspd=arg maximal motor speed (steps per second) - -p, --parse=arg file with SDO data to send to device - -q, --quick directly send command without getting status - -r, --rel=arg move to relative position (steps) - -s, --canspd=arg CAN bus speed - -t, --serialspd=arg serial (tty) device speed (default: 115200) - -u, --microsteps=arg microstepping (0..256) - -w, --wait wait while motor is busy + -0, --zeropos set current position to zero + -A, --disablesw disable end-switches + -D, --disable disable motor + -E, --enablesw=arg enable end-switches with given mask + -I, --nodeid=arg node ID (1..127) + -P, --pidfile=arg pidfile (default: /tmp/steppersmng.pid) + -R, --readvals read values of used parameters + -S, --stop stop motor + -a, --abs=arg move to absolute position (in encoder ticks) + -c, --clearerr clear errors + -d, --device=arg serial device name (default: /dev/ttyUSB0) + -h, --help show this help + -k, --check check SDO data file + -l, --logfile=arg file to save logs + -m, --maxspd=arg maximal motor speed (enc ticks per second) + -p, --parse file[s] with SDO data to send to device + -q, --quick directly send command without getting status + -r, --rel=arg move to relative position (in encoder ticks) + -s, --canspd=arg CAN bus speed + -t, --serialspd=arg serial (tty) device speed (default: 115200) + -u, --microsteps=arg set microstepping (0..256) + -v, --verbose verbosity level for logging (each -v increases level) + -w, --wait wait while motor is busy ## Some usefull information Factory settings of pusirobot drivers: 125kBaud, nodeID=5 + +Speed 6553.6 == 1rev/s diff --git a/commandline/canopen.c b/commandline/canopen.c index 17800d9..18bd5da 100644 --- a/commandline/canopen.c +++ b/commandline/canopen.c @@ -144,7 +144,7 @@ SDO *parseSDO(CANmesg *mesg){ return &sdo; } -// send request to read SDO +// send request to read SDO, return 0 if all OK static int ask2read(uint16_t idx, uint8_t subidx, uint8_t NID){ SDO sdo; sdo.NID = NID; @@ -153,7 +153,10 @@ static int ask2read(uint16_t idx, uint8_t subidx, uint8_t NID){ sdo.index = idx; sdo.subindex = subidx; CANmesg *mesg = mkMesg(&sdo); - return canbus_write(mesg); + int ans = 1; // error + for(int i = 0; i < NTRIES; ++i) + if(!(ans = canbus_write(mesg))) return 0; + return ans; } static SDO *getSDOans(uint16_t idx, uint8_t subidx, uint8_t NID){ @@ -277,7 +280,10 @@ int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){ sdo.subindex = e->subindex; CANmesg *mesgp = mkMesg(&sdo); DBG("Canbus write.."); - if(canbus_write(mesgp)){ + int ans = 1; + for(int i = 0; i < NTRIES; ++i) + if(!(ans = canbus_write(mesgp))) break; + if(ans){ WARNX("SDO_write(): Can't initiate download"); return 2; } diff --git a/commandline/canopen.h b/commandline/canopen.h index 2c01c78..1e26a49 100644 --- a/commandline/canopen.h +++ b/commandline/canopen.h @@ -24,7 +24,9 @@ #include "pusirobot.h" // timeout for answer from the SDO, seconds -#define SDO_ANS_TIMEOUT (1.) +#define SDO_ANS_TIMEOUT (2.5) +// N tries to write CAN message +#define NTRIES (15) // COB-ID base: #define NMT_COBID 0 diff --git a/commandline/cfg/Acceleration.cfg b/commandline/cfg/Acceleration.cfg new file mode 100644 index 0000000..b266f1c --- /dev/null +++ b/commandline/cfg/Acceleration.cfg @@ -0,0 +1,10 @@ +# Transmit SDO to driver +# Format: index, subindex, data + +# Set acceleration (0 - no, 1 - highest, 8 - lowest) +0x6008, 0, 3 +# Set deceleration +0x6009, 0, 3 + +# Save parameters +0x2007, 0, 2 diff --git a/commandline/cfg/NoPDOmapping.cfg b/commandline/cfg/NoPDOmapping.cfg new file mode 100644 index 0000000..04ef790 --- /dev/null +++ b/commandline/cfg/NoPDOmapping.cfg @@ -0,0 +1,2 @@ +# set number of mapped objects to 0 +0x1A00, 0, 0 diff --git a/commandline/cfg/SetCurnt0.6.cfg b/commandline/cfg/SetCurnt0.6.cfg new file mode 100644 index 0000000..56f4b9b --- /dev/null +++ b/commandline/cfg/SetCurnt0.6.cfg @@ -0,0 +1,8 @@ +# Transmit SDO to driver +# Format: index, subindex, data + +# Set max phase current to 600mA +0x600B, 0, 600 + +# Save parameters +0x2007, 0, 2 diff --git a/commandline/cfg/Start_settings.cfg b/commandline/cfg/Start_settings.cfg index 8e9d77d..1b21d96 100644 --- a/commandline/cfg/Start_settings.cfg +++ b/commandline/cfg/Start_settings.cfg @@ -4,16 +4,23 @@ # Set heartbeat time to 0 0x1017, 0, 0 -# Set Node ID to 1 -0x2002, 0, 2 +# Turn off offline programming +0x6018, 1, 0 +0x6018, 2, 0 + +# Set Node ID to 10 +0x2002, 0, 10 # 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 max phase current to 1.5A +0x600B, 0, 1500 + # EXT1&EXT2 emergency stop: EXT1@falling edge (normal state == 1), EXT2@rising edge (normal state == 0) # Falling edge for trigger #1 -0x600F, 2, 0 +#0x600F, 2, 0 # Enable EXT1, EXT2 -0x600F, 1, 1 +#0x600F, 1, 1 # Save parameters 0x2007, 0, 2 diff --git a/commandline/cfg/Start_settingsEXT1.cfg b/commandline/cfg/Start_settingsEXT1.cfg new file mode 100644 index 0000000..d2b6bba --- /dev/null +++ b/commandline/cfg/Start_settingsEXT1.cfg @@ -0,0 +1,27 @@ +# 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) +0x2003, 0, 5 +# Set microstepping to 32 +0x600A, 0, 32 +# Set max phase current to 1.0A +0x600B, 0, 1000 +# Set acceleration (0 - no, 1 - highest, 8 - lowest) +0x6008, 0, 6 +# Set deceleration +0x6009, 0, 7 + +# 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, 1 +# Enable EXT1 +0x600F, 1, 1 + +# Save parameters +0x2007, 0, 2 diff --git a/commandline/cmdlnopts.c b/commandline/cmdlnopts.c index fde34e3..078bcd8 100644 --- a/commandline/cmdlnopts.c +++ b/commandline/cmdlnopts.c @@ -49,7 +49,7 @@ static glob_pars const Gdefault = { .absmove = INT_MIN, .relmove = INT_MIN, .microsteps = -1, - .maxspeed = INT_MIN + .maxspeed = INT_MIN, }; /* @@ -65,22 +65,22 @@ 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)")}, - {"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 (full steps)")}, - {"abs", NEED_ARG, NULL, 'a', arg_int, APTR(&G.absmove), _("move to absolute position (full steps)")}, - {"maxspd", NEED_ARG, NULL, 'm', arg_int, APTR(&G.maxspeed), _("maximal motor speed (full steps per second)")}, + {"microsteps", NEED_ARG,NULL, 'u', arg_int, APTR(&G.microsteps),_("set microstepping (0..256)")}, + {"rel", NEED_ARG, NULL, 'r', arg_int, APTR(&G.relmove), _("move to relative position (in encoder ticks)")}, + {"abs", NEED_ARG, NULL, 'a', arg_int, APTR(&G.absmove), _("move to absolute position (in encoder ticks)")}, + {"maxspd", NEED_ARG, NULL, 'm', arg_int, APTR(&G.maxspeed), _("maximal motor speed (enc ticks 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")}, - {"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")}, + {"parse", MULT_PAR, NULL, 'p', arg_string, APTR(&G.parsefile), _("file[s] with SDO data to send to device")}, + {"check", MULT_PAR, NULL, 'k', arg_string, APTR(&G.checkfile), _("check SDO data file")}, {"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",NEED_ARG, NULL, 'E', arg_int, APTR(&G.enableESW), _("enable end-switches with given mask")}, {"disablesw",NO_ARGS, NULL, 'A', arg_int, APTR(&G.disableESW),_("disable end-switches")}, {"wait", NO_ARGS, NULL, 'w', arg_int, APTR(&G.wait), _("wait while motor is busy")}, {"quick", NO_ARGS, NULL, 'q', arg_int, APTR(&G.quick), _("directly send command without getting status")}, - {"fracsteps",NO_ARGS, NULL, 'F', arg_int, APTR(&G.fracsteps), _("move to steps fractions amount instead of full steps")}, + {"verbose", NO_ARGS, NULL, 'v', arg_none, APTR(&G.verblevel), _("verbosity level for logging (each -v increases level)")}, end_option }; diff --git a/commandline/cmdlnopts.h b/commandline/cmdlnopts.h index be51f27..5acab31 100644 --- a/commandline/cmdlnopts.h +++ b/commandline/cmdlnopts.h @@ -35,13 +35,14 @@ typedef struct{ char *device; // serial device name char *pidfile; // name of PID file char *logfile; // logging to this file - char *parsefile; // file to parse - char *checkfile; // SDO data filename to check + char **parsefile; // file[s] to parse + char **checkfile; // SDO data filename[s] to check + int verblevel; // verbose level for logfile int canspeed; // CAN bus speed int serialspeed; // serial device speed (CAN-bus to USB) int NodeID; // node ID to work with - int absmove; // absolute position to move - int relmove; // relative position to move + int absmove; // absolute position to move (in encoder's ticks) + int relmove; // relative position to move (in encoder's ticks) int microsteps; // set microstepping int maxspeed; // max speed int stop; // stop motor @@ -53,7 +54,6 @@ typedef struct{ int disableESW; // --//-- disable int wait; // wait while device is busy int quick; // directly send command without getting status - int fracsteps; // move in steps fractions instead of full steps } glob_pars; diff --git a/commandline/dataparser.c b/commandline/dataparser.c index 9610771..b13358e 100644 --- a/commandline/dataparser.c +++ b/commandline/dataparser.c @@ -16,15 +16,16 @@ * along with this program. If not, see . */ -#include "canopen.h" -#include "dataparser.h" -#include "pusirobot.h" - #include // basename #include // fopen #include // strchr #include +#include "canopen.h" +#include "dataparser.h" +#include "pusirobot.h" +#include "verblog.h" + #define BUFSZ 256 char *getl(char *str, long long *lp){ @@ -101,7 +102,7 @@ int parse_data_file(const char *fname, uint8_t nid){ } data = (int64_t) l; DBG("Got: idx=0x%04X, subidx=0x%02X, data=0x%lX", idx, sidx, data); - if(nid == 0) printf("line #%d: read SDO with index=0x%04X, subindex=0x%02X, data=0x%lX (dec: %ld)\n", lineno, idx, sidx, data, data); + if(nid == 0) message(1, "line #%d: read SDO with index=0x%04X, subindex=0x%02X, data=0x%lX (dec: %ld)", lineno, idx, sidx, data, data); SDO_dic_entry *entry = dictentry_search(idx, sidx); if(!entry){ WARNX("SDO 0x%04X/0x%02X isn't in dictionary", idx, sidx); @@ -127,7 +128,7 @@ int parse_data_file(const char *fname, uint8_t nid){ } 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); + else message(1, "Send to NID=%d SDO [0x%04X, 0x%02X] with data 0x%lX", nid, entry->index, entry->subindex, data); } }while(0); if(!isgood){ diff --git a/commandline/dicentries.in b/commandline/dicentries.in index 50f57e7..0e5fce1 100644 --- a/commandline/dicentries.in +++ b/commandline/dicentries.in @@ -98,7 +98,7 @@ DICENTRY(MICROSTEPS, 0x600A, 0, 2, 0, "microstepping") // max current DICENTRY(MAXCURNT, 0x600B, 0, 2, 0, "maximum phase current") // current position -DICENTRY(POSITION, 0x600C, 0, 4, 0, "current position") +DICENTRY(POSITION, 0x600C, 0, 4, 1, "current position") // current reduction DICENTRY(CURRREDUCT, 0x600D, 0, 1, 0, "current reduction") // motor enable diff --git a/commandline/main.c b/commandline/main.c index c599bbe..24198eb 100644 --- a/commandline/main.c +++ b/commandline/main.c @@ -28,15 +28,15 @@ #include "cmdlnopts.h" #include "dataparser.h" #include "pusirobot.h" +#include "verblog.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){ - putlog("Exit with status %d", sig); + LOGERR("Exit with status %d", sig); DBG("Exit with status %d", sig); if(GP->pidfile) // remove unnesessary PID file unlink(GP->pidfile); @@ -66,7 +66,7 @@ static inline void chkerr(int64_t es){ // device status check static inline void chkstat(int64_t es){ if(es) red("DEVSTATUS=%d\n", (int)es); - else green("DEVSTATUS=0\n"); + else message(1, "DEVSTATUS=0"); devstat = (uint8_t)es; if(devstat){ for(uint8_t i = 0; i < 8; ++i){ @@ -80,6 +80,7 @@ static inline void chkstat(int64_t es){ } } +/* // setup microstepping static inline void setusteps(int64_t es){ DBG("es=%zd", es); @@ -94,7 +95,7 @@ static inline void setusteps(int64_t es){ } microstepping = es > 0 ? (uint16_t) es : 1; green("MICROSTEPPING=%u\n", microstepping); -} +}*/ // setup maximal speed static inline void setmaxspd(int64_t es){ @@ -102,14 +103,14 @@ static inline void setmaxspd(int64_t es){ 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); + GP->maxspeed *= SPEED_MULTIPLIER; + if(GP->maxspeed < MAX_SPEED_MIN|| GP->maxspeed > MAX_SPEED_MAX) + ERRX("MAXSPEED should be from %d to %d", MAX_SPEED_MIN / SPEED_MULTIPLIER, MAX_SPEED_MAX / SPEED_MULTIPLIER); 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); + if(es) message(1, "MAXSPEED=%d", (int)es); else red("MAXSPEED=0\n"); } @@ -172,17 +173,17 @@ int main(int argc, char *argv[]){ initial_setup(); char *self = strdup(argv[0]); GP = parse_args(argc, argv); + if(GP->verblevel) maxmesglevl(GP->verblevel); if(GP->checkfile){ // just check and exit - return parse_data_file(GP->checkfile, 0); + char **c = GP->checkfile; + int r = 0; + while(*c){ + message(1, "\nCheck data file %s", *c); + r += parse_data_file(*c, 0); + ++c; + } + return r; } - check4running(self, GP->pidfile); - free(self); - signal(SIGTERM, signals); // kill (-15) - quit - signal(SIGHUP, SIG_IGN); // hup - ignore - 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"); } @@ -195,45 +196,55 @@ int main(int argc, char *argv[]){ if(GP->maxspeed == 0) ERRX("Set non-zero MAXSPEED"); } + if(GP->enableESW && GP->disableESW) ERRX("Enable & disable ESW can't meet together"); - if(GP->logfile) openlogfile(GP->logfile); - putlog(("Start application...")); - putlog("Try to open CAN bus device %s", GP->device); + check4running(self, GP->pidfile); + free(self); + signal(SIGTERM, signals); // kill (-15) - quit + signal(SIGHUP, SIG_IGN); // hup - ignore + signal(SIGINT, signals); // ctrl+C - quit + signal(SIGQUIT, signals); // ctrl+\ - quit + signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z + + if(GP->logfile){ + sl_loglevel l = LOGLVL + GP->verblevel; + if(l > LOGLEVEL_ANY) l = LOGLEVEL_ANY; + OPENLOG(GP->logfile, l, 1); + LOGMSG(("Start application...")); + LOGMSG("Try to open CAN bus device %s", GP->device); + } setserialspeed(GP->serialspeed); if(canbus_open(GP->device)){ - putlog("Can't open %s @ speed %d. Exit.", GP->device, GP->serialspeed); - ERRX("Can't open %s @ speed %d. Exit.", GP->device, GP->serialspeed); + LogAndErr("Can't open %s @ speed %d. Exit.", GP->device, GP->serialspeed); } if(canbus_setspeed(GP->canspeed)){ - putlog("Can't set CAN speed %d. Exit.", GP->canspeed); - ERRX("Can't set CAN speed %d. Exit.", GP->canspeed); + LogAndErr("Can't set CAN speed %d. Exit.", GP->canspeed); } // print current position and state int64_t i64; 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) +#define getSDOe(SDO, fn, e) do{if(INT64_MIN != (i64 = SDO_read(&SDO, ID))) fn(i64); else LogAndWarn(e); }while(0) +#define getSDOw(SDO, fn, e) do{if(INT64_MIN != (i64 = SDO_read(&SDO, ID))) fn(i64); else LogAndErr(e); }while(0) #define Mesg(...) //#define Mesg(...) green(__VA_ARGS__) //double d0 = dtime(); // get mircostepping (need this to properly calculate current position and move - if(GP->fracsteps) microstepping = 1; // Don't calculate microstepping in this case - else{ + /*if(!GP->fracsteps){ getSDOe(MICROSTEPS, setusteps, "Can't get microstepping"); Mesg("MICROSTEPS: %g\n", dtime() - d0); - } + }*/ if(!GP->quick){ // check error status getSDOe(ERRSTATE, chkerr, "Can't get error status"); Mesg("ERRSTATE: %g\n", dtime() - d0); // get current position if(INT64_MIN != (i64 = SDO_read(&POSITION, ID))){ - int enc = (int)i64; - green("CURENCODER=%d\n", enc); - green("CURSTEPS=%.2f\n", enc / ((double)microstepping)); + //int enc = (int)i64; + message(1, "CURENCODER=%d", i64); + //message(1, "CURSTEPS=%.2f", i64 / ((double)microstepping)); } else WARNX("Can't read current position"); Mesg("CURPOS: %g\n", dtime() - d0); @@ -242,14 +253,20 @@ int main(int argc, char *argv[]){ Mesg("GPIOVAL: %g\n", dtime() - d0); // get motor power status if(INT64_MIN != (i64 = SDO_read(&ENABLE, ID))){ - if(i64) green("ENABLE=1\n"); - else red("ENABLE=0\n"); + if(i64) message(1, "ENABLE=1"); + else message(1, "ENABLE=0"); Mesg("Status: %g\n", dtime() - d0); } // get max speed getSDOe(MAXSPEED, setmaxspd, "Can't read max speed"); Mesg("MAXSPEED: %g\n", dtime() - d0); + i64 = SDO_read(&MICROSTEPS, ID); + if(i64 == INT64_MIN) LogAndWarn("Can't get microstepping value"); + else message(2, "MICROSTEPS=%u", i64); } + i64 = SDO_read(&ENCRESOL, ID); + if(i64 == INT64_MIN) LogAndWarn("Can't get encoder resolution value"); + else message(2, "ENCRESOL=%u", 1 << i64); if(GP->absmove != INT_MIN || GP->relmove != INT_MIN || !GP->quick || GP->wait){ // check device status getSDOe(DEVSTATUS, chkstat, "Can't get device status"); @@ -279,9 +296,13 @@ int main(int argc, char *argv[]){ } // send values from external configuration file 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); + char **p = GP->parsefile; + while(*p){ + message(1, "Try to parse %s and send SDOs to device", *p); + parse_data_file(*p, GP->NodeID); + Mesg("parse_data_file: %g\n", dtime() - d0); + ++p; + } } // enable limit switches if(GP->enableESW){ @@ -300,12 +321,11 @@ int main(int argc, char *argv[]){ } //int64_t es = SDO_read(&EXTENABLE, ID); //green("LIMITSW=%lld\n", es); - int multiplier = GP->fracsteps ? 1 : microstepping; // move to absolute position 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 * multiplier)) + if(SDO_write(&ABSSTEPS, ID, GP->absmove)) ERRX("Can't move to absolute position %d", GP->absmove); } if(GP->relmove != INT_MIN && GP->relmove){ @@ -318,8 +338,7 @@ int main(int argc, char *argv[]){ } 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 * multiplier)) + if(SDO_write(&RELSTEPS, ID, GP->relmove)) ERRX("Can't move to relative position %d", GP->relmove); Mesg("RelMove: %g\n", dtime() - d0); } diff --git a/commandline/pusirobot.h b/commandline/pusirobot.h index af22e22..6ad6589 100644 --- a/commandline/pusirobot.h +++ b/commandline/pusirobot.h @@ -39,8 +39,11 @@ typedef struct{ extern const int DEsz; extern const SDO_dic_entry* allrecords[]; +// speed limits #define MAX_SPEED_MIN -200000 #define MAX_SPEED_MAX 200000 +// encoder ticks per rev * SPEED_MULTIPLIER = speed for 1 rev per second +#define SPEED_MULTIPLIER (1.6) // limit switches mask in GPIO status register (x=1,2,3) #define EXTMASK(x) (1<<(6+x)) diff --git a/commandline/verblog.c b/commandline/verblog.c new file mode 100644 index 0000000..b3de45c --- /dev/null +++ b/commandline/verblog.c @@ -0,0 +1,38 @@ +/* + * This file is part of the stepper project. + * Copyright 2022 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "verblog.h" +#include +#include +#include + +// show messages only with level <= maxlevel +static int maxlevel = MAXVERBLVL; + +void message(int level, const char *fmt, ...){ + if(level > maxlevel) return; + va_list ar; + va_start(ar, fmt); + vprintf(fmt, ar); + va_end(ar); + printf("\n"); +} + +void maxmesglevl(int level){ + maxlevel = level; +} diff --git a/commandline/verblog.h b/commandline/verblog.h new file mode 100644 index 0000000..8f825bc --- /dev/null +++ b/commandline/verblog.h @@ -0,0 +1,32 @@ +/* + * This file is part of the stepper project. + * Copyright 2022 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#ifdef EBUG +#define MAXVERBLVL 1 +#define LOGLVL LOGLEVEL_DBG +#else +#define MAXVERBLVL 0 +#define LOGLVL LOGLEVEL_WARN +#endif + +#define LogAndWarn(...) do{LOGWARN(__VA_ARGS__); WARNX(__VA_ARGS__);}while(0) +#define LogAndErr(...) do{LOGERR(__VA_ARGS__); ERRX(__VA_ARGS__);}while(0) +void message(int level, const char *fmt, ...); +void maxmesglevl(int level);