Add better reaction to -q keyword

commandline/canbus.c

@@ -54,7 +54,6 @@ 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;
@@ -179,7 +178,6 @@ 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;
@@ -252,7 +250,6 @@ void showM(CANmesg *m){
 #endif
 
 int canbus_read(CANmesg *mesg){
-    FNAME();
     if(!mesg) return 1;
     pthread_mutex_lock(&mutex);
     double t0 = dtime();

commandline/cfg/Oscill

@@ -0,0 +1,22 @@
+#!/bin/bash
+
+Ampl=$1
+
+./steppermove -s250
+./steppermove -I1 -m4000
+./steppermove -I2 -m4000
+
+#if false; then
+./steppermove -I2 -qr-3000
+./steppermove -I1 -qr-3000
+./steppermove -I2 -qw -r500
+./steppermove -I1 -qr500
+./steppermove -I2 -qw
+#fi
+
+while true; do
+./steppermove -I2 -qwr${Ampl}
+./steppermove -I1 -qr${Ampl}
+./steppermove -I2 -qwr-${Ampl}
+./steppermove -I1 -qr-${Ampl}
+done

commandline/cfg/SetCurnt0.8.cfg

@@ -0,0 +1,8 @@
+# Transmit SDO to driver
+# Format: index, subindex, data
+
+# Set max phase current to 800mA
+0x600B, 0, 800
+
+# Save parameters
+0x2007, 0, 2

commandline/cfg/SetCurnt1.0.cfg

@@ -0,0 +1,8 @@
+# Transmit SDO to driver
+# Format: index, subindex, data
+
+# Set max phase current to 600mA
+0x600B, 0, 1000
+
+# Save parameters
+0x2007, 0, 2

commandline/cfg/SetCurnt1.5.cfg

@@ -0,0 +1,8 @@
+# Transmit SDO to driver
+# Format: index, subindex, data
+
+# Set max phase current to 600mA
+0x600B, 0, 1500
+
+# Save parameters
+0x2007, 0, 2

commandline/cfg/SetCurnt2.0.cfg

@@ -0,0 +1,8 @@
+# Transmit SDO to driver
+# Format: index, subindex, data
+
+# Set max phase current to 2A
+0x600B, 0, 2000
+
+# Save parameters
+0x2007, 0, 2

commandline/cfg/TurretMove

@@ -0,0 +1,37 @@
+#!/bin/bash
+
+function move(){
+	./steppermove -I4 $*
+}
+
+if [ $# -ne 1 ]; then
+	echo "Usage: $0 Nposition (enumeration from 0)" >&2
+	exit 1
+fi
+
+if [ $1 -lt 0 -o $1 -gt 5 ]; then
+	echo "Turet position should be from 0 to 5" >&2
+	exit 2
+fi
+
+LAST=-1
+[ -f lastpos ] && LAST=$(cat lastpos)
+
+eval "$(move -s250 -m50)"
+
+DIFF=$((CURENCODER - LAST))
+ABS=${DIFF#-}  # absoulte value
+
+if [ $ABS -gt 3 ]; then # goto zero if last position differs more then for 3 from current
+	move -cE1 -r-1000
+	move -wcA
+	move -0
+fi
+
+move -Fa $((683*$1))
+move -w
+
+sleep 1 # wait for stopping
+eval "$(move)"
+echo $CURENCODER > lastpos
+echo "Current encoder position: $CURENCODER"

commandline/cmdlnopts.c

@@ -66,9 +66,9 @@ static myoption cmdlnopts[] = {
     {"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 (steps)")},
+    {"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 (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 (steps per second)")},
+    {"maxspd",  NEED_ARG,   NULL,   'm',    arg_int,    APTR(&G.maxspeed),  _("maximal motor speed (full 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")},
@@ -76,9 +76,11 @@ static myoption cmdlnopts[] = {
     {"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")},
     {"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")},
+    {"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")},
    end_option
 };
 

commandline/cmdlnopts.h

@@ -50,8 +50,10 @@ typedef struct{
     int disable;            // disable motor
     int showpars;           // show values of some parameters
     int enableESW;          // send signal to enable end-switches
+    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;
 
 

commandline/main.c

@@ -82,12 +82,17 @@ static inline void chkstat(int64_t es){
 
 // setup microstepping
 static inline void setusteps(int64_t es){
-    if(GP->microsteps > -1 && GP->microsteps != (int) es){
+    DBG("es=%zd", es);
+    int us = GP->microsteps;
+    if(us > 0){
+        if(us == 1) us = 0; // PusiRobot driver needs value 0 for microstepping 1!
+        if(us != (int) es){ // change microstepping
         DBG("Try to change microsteps");
-        if(SDO_write(&MICROSTEPS, ID, GP->microsteps) || INT64_MIN == (es = SDO_read(&MICROSTEPS, ID)))
+        if(SDO_write(&MICROSTEPS, ID, us) || INT64_MIN == (es = SDO_read(&MICROSTEPS, ID)))
             ERRX("Can't change microstepping");
+        }
     }
-    microstepping = (uint16_t) es;
+    microstepping = es > 0 ? (uint16_t) es : 1;
     green("MICROSTEPPING=%u\n", microstepping);
 }
 
@@ -181,8 +186,9 @@ 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
+    DBG("builtin: %d", __builtin_popcount(GP->microsteps));
-        ERRX("Wrong microstepping settings, should be 0 or 2^(1..8)");
+    if(GP->microsteps > 0 && (1 != __builtin_popcount(GP->microsteps))) // __builtin_popcount - amount of non-zero bits in uint
+        ERRX("Wrong microstepping settings, should be 2^(0..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");
@@ -214,15 +220,21 @@ int main(int argc, char *argv[]){
 //double d0 = dtime();
 
     // get mircostepping (need this to properly calculate current position and move
-    getSDOe(MICROSTEPS, setusteps, "Can't get microstepping");
+    if(GP->fracsteps) microstepping = 1; // Don't calculate microstepping in this case
-    Mesg("MICROSTEPS: %g\n", dtime() - d0);
+    else{
+        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)))
+        if(INT64_MIN != (i64 = SDO_read(&POSITION, ID))){
-            green("CURPOS=%d\n", (int)i64/microstepping);
+            int enc = (int)i64;
+            green("CURENCODER=%d\n", enc);
+            green("CURSTEPS=%.2f\n", enc / ((double)microstepping));
+        }
         else WARNX("Can't read current position");
         Mesg("CURPOS: %g\n", dtime() - d0);
         // get limit switches values
@@ -238,10 +250,12 @@ int main(int argc, char *argv[]){
         getSDOe(MAXSPEED, setmaxspd, "Can't read max speed");
         Mesg("MAXSPEED: %g\n", dtime() - d0);
     }
-    // check device status
+    if(GP->absmove != INT_MIN || GP->relmove != INT_MIN || !GP->quick || GP->wait){
-    getSDOe(DEVSTATUS, chkstat, "Can't get device status");
+        // check device status
-    Mesg("DEVSTATUS: %g\n", dtime() - d0);
+        getSDOe(DEVSTATUS, chkstat, "Can't get device status");
-    if(devstat == BUSY_STATE && GP->wait) wait_busy();
+        Mesg("DEVSTATUS: %g\n", dtime() - d0);
+        if(devstat == BUSY_STATE && GP->wait) wait_busy();
+    }
     // stop motor
     if(GP->stop){
         if(SDO_write(&STOP, ID, 1)) ERRX("Can't stop motor");
@@ -271,19 +285,28 @@ int main(int argc, char *argv[]){
     }
     // enable limit switches
     if(GP->enableESW){
-        if(SDO_write(&EXTENABLE, ID, 3)){
+        if(SDO_write(&EXTENABLE, ID, GP->enableESW)){
             WARNX("Error when trying to enable limit switches");
             if(GP->absmove || GP->relmove) signals(-1);
         }
         Mesg("EXTENABLE: %g\n", dtime() - d0);
     }
+    // disable limit switches
+    if(GP->disableESW){
+        if(SDO_write(&EXTENABLE, ID, 0)){
+            WARNX("Error when trying to disable limit switches");
+            if(GP->absmove || GP->relmove) signals(-1);
+        }
+    }
+    //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*microstepping))
+        if(SDO_write(&ABSSTEPS, ID, GP->absmove * multiplier))
             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");
@@ -296,7 +319,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*microstepping))
+        if(SDO_write(&RELSTEPS, ID, GP->relmove * multiplier))
             ERRX("Can't move to relative position %d", GP->relmove);
         Mesg("RelMove: %g\n", dtime() - d0);
     }