a first approach to CLI

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)

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);

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;
+    return getSDOans(idx, subidx, NID);
-    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;
 }
 
 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

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__

commandline/cmdlnopts.c

@@ -25,6 +25,7 @@
 #include <usefull_macros.h>
 
 #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")},
+    {"microsteps", NEED_ARG,NULL,   'u',    arg_int,    APTR(&G.microsteps),_("microstepping (0..256)")},
-    {"abs",     NEED_ARG,   NULL,   'r',    arg_int,    APTR(&G.absmove),   _("move to absolute position")},
+    {"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
 };
 

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;
 
 

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)))
+        uint8_t s = (uint8_t)i64;
-        green("CURPOS=%d\n", (int)i64);
+        if(s){
-    if(INT64_MIN != (i64 = SDO_read(&MAXSPEED, GP->NodeID)))
+            for(uint8_t i = 0; i < 8; ++i){
-        green("MAXSPEED=%d\n", (int)i64);
+                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.;

commandline/pusirobot.c

@@ -16,12 +16,48 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-//#include <limits.h>
+#include <stdlib.h> // 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<<bit)) return DevStatus[bit];
+    return NULL;
+}
+
+// error codes explanation
+const char *errname(uint8_t error, uint8_t bit){
+    if(bit > 5) return NULL;
+    if(error & (1<<bit)) return DevError[bit];
+    return NULL;
+}
+
 /*
 // get current position for node ID `NID`, @return INT_MIN if error
 int get_current_position(uint8_t NID){

commandline/pusirobot.h

@@ -69,9 +69,18 @@ DICENTRY(MAXCURNT,      0x600B, 0, 2, 0)
 DICENTRY(POSITION,      0x600C, 0, 4, 0)
 // motor enable
 DICENTRY(ENABLE,        0x600E, 0, 1, 0)
+// absolute displacement
+DICENTRY(ABSSTEPS,      0x601C, 0, 4, 1)
+// stop motor
+DICENTRY(STOP,          0x6020, 0, 1, 0)
 
+#define MAX_SPEED_MIN   -200000
+#define MAX_SPEED_MAX   200000
 
-
+// unclearable status
+#define BUSY_STATE      (1<<3)
+const char *devstatus(uint8_t status, uint8_t bit);
+const char *errname(uint8_t error, uint8_t bit);
 //int get_current_position(uint8_t NID);
 
 #endif // PUSIROBOT_H__