Add config-file parser (read SDOs from file and send them to driver)

commandline/Start_settings.cfg

@@ -0,0 +1,24 @@
+# Transmit SDO to driver
+# Format: index, subindex, data
+
+# Set Node ID to 1
+0x2002, 0, 1
+# Set microstepping to 32
+0x600A, 0, 32
+# Set max phase current to 600mA
+0x600B, 0, 600
+# Set acceleration (0 - no, 1 - highest, 8 - lowest)
+0x6008, 0, 4
+# 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
+
+# Save parameters
+0x2007, 0, 2

commandline/canbus.c

@@ -54,7 +54,7 @@ static char *read_string();
  * @return amount of bytes read
  */
 static int read_ttyX(TTY_descr *d){
-    if(d->comfd < 0) return 0;
+    if(!d || d->comfd < 0) return -1;
     size_t L = 0;
     ssize_t l;
     size_t length = d->bufsz;

commandline/cmdlnopts.c

@@ -64,7 +64,7 @@ static myoption cmdlnopts[] = {
     {"serialspd",NEED_ARG,  NULL,   't',    arg_int,    APTR(&G.serialspeed),_("serial (tty) device speed (default: " STR(DEFAULT_SPEED) ")")},
     {"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)")},
+    {"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 (steps)")},
     {"abs",     NEED_ARG,   NULL,   'a',    arg_int,    APTR(&G.absmove),   _("move to absolute position (steps)")},
@@ -72,6 +72,8 @@ static myoption cmdlnopts[] = {
     {"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")},
    end_option
 };
 

commandline/cmdlnopts.h

@@ -35,6 +35,7 @@ typedef struct{
     char *device;           // serial device name
     char *pidfile;          // name of PID file
     char *logfile;          // logging to this file
+    char *parsefile;        // file to parse
     int canspeed;           // CAN bus speed
     int serialspeed;        // serial device speed (CAN-bus to USB)
     int NodeID;             // node ID to work with
@@ -45,6 +46,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
 } glob_pars;
 
 

commandline/dataparser.c

@@ -0,0 +1,138 @@
+/*
+ * This file is part of the stepper project.
+ * Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
+ *
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#include "canopen.h"
+#include "dataparser.h"
+#include "pusirobot.h"
+
+#include <libgen.h> // basename
+#include <stdio.h>  // fopen
+#include <string.h> // strchr
+#include <usefull_macros.h>
+
+#define BUFSZ 256
+
+char *getl(char *str, long long *lp){
+    long long l;
+    char *eptr;
+    int base = 0;
+    if(str[0] == '0' && (str[1] == 'b' || str[1] == 'B')){ // binary number format
+        base = 2;
+        str += 2;
+    }
+    l = strtoll(str, &eptr, base);
+    if(str == eptr) return NULL;
+    if(lp) *lp = l;
+    return eptr;
+}
+
+// get next value - after ',' and spaces
+char *getnxt(char *str){
+    char *comma = strchr(str, ',');
+    if(!comma || !*comma) return NULL;
+    for(; str < comma; ++str) if(*str > ' ') return NULL;
+    ++comma;
+    while(*comma && *comma <= ' ') ++comma;
+    return comma;
+}
+
+/**
+ * @brief parse_data_file - read SDO & data from file and send them to node `nid`
+ * @param fname - file name
+ * @param nid   - node ID or 0 to check file
+ * @return 0 if no errors found
+ */
+int parse_data_file(const char *fname, uint8_t nid){
+    int ret = 0;
+    if(!fname){
+        WARNX("No filename for parsing");
+        return 1;
+    }
+    FILE *f = fopen(fname, "r");
+    if(!f){
+        WARN("Can't open %s for parsing", basename((char*)fname));
+        return 2;
+    }
+    char str[BUFSZ];
+    int lineno = 0;
+    while(fgets(str, BUFSZ, f)){
+        char *num = strchr(str, '#');
+        if(num) *num = 0;
+        long long l;
+        uint16_t idx;
+        uint8_t sidx;
+        int64_t data;
+        int isgood = 1;
+        do{
+            char *ptr;
+            if(!(ptr = getl(str, &l))) break;
+            idx = (uint16_t) l;
+            if(!(ptr = getnxt(ptr))){
+                isgood = 0;
+                break;
+            }
+            if(!(ptr = getl(ptr, &l))){
+                isgood = 0;
+                break;
+            }
+            sidx = (uint8_t) l;
+            if(!(ptr = getnxt(ptr))){
+                isgood = 0;
+                break;
+            }
+            if(!(ptr = getl(ptr, &l))){
+                isgood = 0;
+                break;
+            }
+            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);
+            SDO_dic_entry *entry = dictentry_search(idx, sidx);
+            if(!entry){
+                WARNX("SDO 0x%04X/0x%02X isn't in dictionary", idx, sidx);
+                continue;
+            }
+            if(data<0 && !entry->issigned){
+                WARNX("SDO 0x%04X/0x%02X is only positive", idx, sidx);
+                continue;
+            }
+            uint64_t u64 = (uint64_t) (l > 0) ? l : -l;
+            if(u64 > UINT32_MAX){
+                WARNX("The data size of SDO 0x%04X/0x%02X out of possible range for uint32_t", idx, sidx);
+                continue;
+            }
+            uint32_t u = (uint32_t) (l > 0) ? l : -l;
+            if(entry->datasize != 4){
+                uint32_t mask = 0xff;
+                if(entry->datasize == 2) mask = 0xffff;
+                if((u & mask) != u){
+                    WARNX("The data size of SDO 0x%04X/0x%02X is larger than possible (%d byte[s])", idx, sidx, entry->datasize);
+                    continue;
+                }
+            }
+            if(nid) SDO_write(entry, nid, 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);
+        }
+        ++lineno;
+    }
+    fclose(f);
+    return ret;
+}
+

commandline/dataparser.h

@@ -0,0 +1,27 @@
+/*
+ * This file is part of the stepper project.
+ * Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
+ *
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+#ifndef DATAPARSER_H__
+#define DATAPARSER_H__
+
+#include <stdint.h>
+
+int parse_data_file(const char *fname, uint8_t nid);
+
+#endif // DATAPARSER_H__

commandline/dicentries.h

@@ -0,0 +1,67 @@
+/*
+ * This file is part of the stepper project.
+ * Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
+ *
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+// this file can be included more than once!
+
+// heartbeat time
+DICENTRY(HEARTBTTIME,   0x1017, 0, 2, 0)
+// node ID
+DICENTRY(NODEID,        0x2002, 0, 1, 0)
+// baudrate
+DICENTRY(BAUDRATE,      0x2003, 0, 1, 0)
+// system control: 1- bootloader, 2 - save parameters, 3 - reset factory settings
+DICENTRY(SYSCONTROL,    0x2007, 0, 1, 0)
+// error state
+DICENTRY(ERRSTATE,      0x6000, 0, 1, 0)
+// controller status
+DICENTRY(DEVSTATUS,     0x6001, 0, 1, 0)
+// rotation direction
+DICENTRY(ROTDIR,        0x6002, 0, 1, 0)
+// maximal speed
+DICENTRY(MAXSPEED,      0x6003, 0, 4, 1)
+// relative displacement
+DICENTRY(RELSTEPS,      0x6004, 0, 4, 0)
+// operation mode
+DICENTRY(OPMODE,        0x6005, 0, 1, 0)
+// start speed
+DICENTRY(STARTSPEED,    0x6006, 0, 2, 0)
+// stop speed
+DICENTRY(STOPSPEED,     0x6007, 0, 2, 0)
+// acceleration coefficient
+DICENTRY(ACCELCOEF,     0x6008, 0, 1, 0)
+// deceleration coefficient
+DICENTRY(DECELCOEF,     0x6009, 0, 1, 0)
+// microstepping
+DICENTRY(MICROSTEPS,    0x600A, 0, 2, 0)
+// max current
+DICENTRY(MAXCURNT,      0x600B, 0, 2, 0)
+// current position
+DICENTRY(POSITION,      0x600C, 0, 4, 0)
+// motor enable
+DICENTRY(ENABLE,        0x600E, 0, 1, 0)
+// EXT emergency stop enable
+DICENTRY(EXTENABLE,     0x600F, 1, 1, 0)
+// EXT emergency stop trigger mode
+DICENTRY(EXTTRIGMODE,   0x600F, 2, 1, 0)
+// EXT emergency sensor type
+DICENTRY(EXTSENSTYPE,   0x600F, 3, 1, 0)
+// absolute displacement
+DICENTRY(ABSSTEPS,      0x601C, 0, 4, 1)
+// stop motor
+DICENTRY(STOP,          0x6020, 0, 1, 0)
+

commandline/main.c

@@ -16,6 +16,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include "dataparser.h"
 #include <signal.h>
 #include <stdio.h>
 #include <string.h>
@@ -48,6 +49,9 @@ int main(int argc, char *argv[]){
     initial_setup();
     char *self = strdup(argv[0]);
     GP = parse_args(argc, argv);
+    if(GP->checkfile){ // just check and exit
+        return parse_data_file(GP->checkfile, 0);
+    }
     check4running(self, GP->pidfile);
     free(self);
     signal(SIGTERM, signals); // kill (-15) - quit
@@ -80,6 +84,11 @@ int main(int argc, char *argv[]){
         signals(2);
     }
 
+    if(GP->parsefile){
+        green("Try to parse %s and send SDOs to device\n", GP->parsefile);
+        parse_data_file(GP->parsefile, GP->NodeID);
+    }
+
     //setup_con();
     // print current position and state
     int64_t i64;

commandline/pusirobot.c

@@ -18,10 +18,21 @@
 
 #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"
 
+// we should init constants here!
+#undef DICENTRY
+#define DICENTRY(name, idx, sidx, sz, s)  const SDO_dic_entry name = {idx, sidx, sz, s};
+#include "dicentries.h"
+
+// now init array with all dictionary
+#undef DICENTRY
+#define DICENTRY(name, idx, sidx, sz, s)  {idx, sidx, sz, s},
+static const SDO_dic_entry allrecords[] = {
+#include "dicentries.h"
+};
+const int DEsz = sizeof(allrecords) / sizeof(SDO_dic_entry);
+
 // controller status for bits
 static const char *DevStatus[] = {
     "External stop 1",
@@ -58,12 +69,12 @@ const char *errname(uint8_t error, uint8_t bit){
     return NULL;
 }
 
-/*
+// search if the object exists in dictionary - for config file parser
-// get current position for node ID `NID`, @return INT_MIN if error
+SDO_dic_entry *dictentry_search(uint16_t index, uint8_t subindex){
-int get_current_position(uint8_t NID){
+    // the search is linear as dictionary can be unsorted!!!
-    int64_t val = SDO_read(&POSITION, NID);
+    for(int i = 0; i < DEsz; ++i){
-    if(val == INT64_MIN) return INT_MIN;
+        const SDO_dic_entry *entry = &allrecords[i];
-    return (int) val;
+        if(entry->index == index && entry->subindex == subindex) return (SDO_dic_entry*)entry;
+    }
+    return NULL;
 }
-*/
-

commandline/pusirobot.h

@@ -33,46 +33,7 @@ typedef struct{
 #define DICENTRY(name, idx, sidx, sz, s)  extern const SDO_dic_entry name;
 #endif
 
-// heartbeat time
+#include "dicentries.h"
-DICENTRY(HEARTBTTIME,   0x1017, 0, 2, 0)
-// node ID
-DICENTRY(NODEID,        0x2002, 0, 1, 0)
-// baudrate
-DICENTRY(BAUDRATE,      0x2003, 0, 1, 0)
-// system control: 1- bootloader, 2 - save parameters, 3 - reset factory settings
-DICENTRY(SYSCONTROL,    0x2007, 0, 1, 0)
-// error state
-DICENTRY(ERRSTATE,      0x6000, 0, 1, 0)
-// controller status
-DICENTRY(DEVSTATUS,     0x6001, 0, 1, 0)
-// rotation direction
-DICENTRY(ROTDIR,        0x6002, 0, 1, 0)
-// maximal speed
-DICENTRY(MAXSPEED,      0x6003, 0, 4, 1)
-// relative displacement
-DICENTRY(RELSTEPS,      0x6004, 0, 4, 0)
-// operation mode
-DICENTRY(OPMODE,        0x6005, 0, 1, 0)
-// start speed
-DICENTRY(STARTSPEED,    0x6006, 0, 2, 0)
-// stop speed
-DICENTRY(STOPSPEED,     0x6007, 0, 2, 0)
-// acceleration coefficient
-DICENTRY(ACCELCOEF,     0x6008, 0, 1, 0)
-// deceleration coefficient
-DICENTRY(DECELCOEF,     0x6009, 0, 1, 0)
-// microstepping
-DICENTRY(MICROSTEPS,    0x600A, 0, 2, 0)
-// max current
-DICENTRY(MAXCURNT,      0x600B, 0, 2, 0)
-// current position
-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
@@ -81,6 +42,7 @@ DICENTRY(STOP,          0x6020, 0, 1, 0)
 #define BUSY_STATE      (1<<3)
 const char *devstatus(uint8_t status, uint8_t bit);
 const char *errname(uint8_t error, uint8_t bit);
+SDO_dic_entry *dictentry_search(uint16_t index, uint8_t subindex);
 //int get_current_position(uint8_t NID);
 
 #endif // PUSIROBOT_H__