some changes: bug fixes, working with PMC007

commandline/CMakeLists.txt

@@ -1,11 +1,11 @@
 cmake_minimum_required(VERSION 3.0)
 set(PROJ steppermove)
-set(MINOR_VERSION "1")
+set(MINOR_VERSION "2")
 set(MID_VERSION "0")
 set(MAJOR_VERSION "0")
 set(VERSION "${MAJOR_VERSION}.${MID_VERSION}.${MINOR_VERSION}")
 
-project(${PROJ} VERSION ${PROJ_VERSION} LANGUAGES C)
+project(${PROJ} VERSION ${VERSION} LANGUAGES C)
 #enable_language(C)
 
 message("VER: ${VERSION}")

commandline/canbus.c

@@ -98,6 +98,7 @@ static int ttyWR(const char *buff, int len){
     int w = write_tty(dev->comfd, buff, (size_t)len);
     if(!w) w = write_tty(dev->comfd, "\n", 1);
     DBG("Written, dt=%g", dtime() - t0);
+    /*
     int errctr = 0;
     while(1){
         char *s = read_string(); // clear echo & check
@@ -108,7 +109,7 @@ static int ttyWR(const char *buff, int len){
                 break;
             }
         }else break;
-    }
+    }*/
     pthread_mutex_unlock(&mutex);
     DBG("Success, dt=%g", dtime() - t0);
     return w;
@@ -206,7 +207,7 @@ static char *read_string(){
             memcpy(ptr, dev->buf, dev->buflen);
             r += l; LL -= l; ptr += l;
             if(ptr[-1] == '\n'){
-                //DBG("Newline detected");
+                DBG("Newline detected");
                 break;
             }
             d0 = dtime();
@@ -218,6 +219,7 @@ static char *read_string(){
         if(optr){
             *optr = 0;
             ++optr;
+            if(*optr) DBG("optr=%s", optr);
         }else{
             WARNX("read_string(): no newline found");
             DBG("buf: %s", buf);

commandline/canbus.h

@@ -22,7 +22,7 @@
 #include <stdint.h>
 
 #ifndef T_POLLING_TMOUT
-#define T_POLLING_TMOUT (0.5)
+#define T_POLLING_TMOUT (0.1)
 #endif
 
 typedef struct{

commandline/canopen.c

@@ -164,17 +164,21 @@ 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){
+    while(dtime() - t0 < T_POLLING_TMOUT){
         mesg.ID = TSDO_COBID | NID; // read only from given ID
         if(canbus_read(&mesg)){
+            DBG("NO CAN data");
             continue;
         }
         sdo = parseSDO(&mesg);
-        if(!sdo) continue;
+        if(!sdo){
+            DBG("Not 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);
+        DBG("No answer from SDO 0x%X/0x%X", idx, subidx);
         return NULL;
     }
     return sdo;
@@ -189,11 +193,18 @@ static SDO *getSDOans(uint16_t idx, uint8_t subidx, uint8_t NID){
  */
 SDO *readSDOvalue(uint16_t idx, uint8_t subidx, uint8_t NID){
     FNAME();
-    if(ask2read(idx, subidx, NID)){
+    double t0 = dtime();
-        WARNX("readSDOvalue(): Can't initiate upload");
+    SDO *sdo = NULL;
-        return NULL;
+    for(int i = 0; i < NTRIES && dtime() - t0 < SDO_ANS_TIMEOUT; ++i){
+        DBG("Try %d ...", i);
+        if(ask2read(idx, subidx, NID)){
+            DBG("Can't initiate upload");
+            continue;
+        }
+        if((sdo = getSDOans(idx, subidx, NID))) break;
+        DBG("got no answer");
     }
-    return getSDOans(idx, subidx, NID);
+    return sdo;
 }
 
 static inline uint32_t mku32(uint8_t data[4]){
@@ -228,10 +239,12 @@ int64_t SDO_read(const SDO_dic_entry *e, uint8_t NID){
         return INT64_MIN;
     }
     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);
+        if(ac != 0x06020000){ // don't warn for unexistant objects
-        if(etxt) WARNX("Abort code 0x%X: %s", ac, etxt);
+            WARNX("Got error for SDO 0x%X", e->index);
+            const char *etxt = abortcode_text(ac);
+            if(etxt) WARNX("Abort code 0x%X: %s", ac, etxt);
+        }
         return INT64_MIN;
     }
     if(sdo->datalen != e->datasize){

commandline/canopen.h

@@ -24,9 +24,9 @@
 #include "pusirobot.h"
 
 // timeout for answer from the SDO, seconds
-#define SDO_ANS_TIMEOUT     (2.5)
+#define SDO_ANS_TIMEOUT     (0.2)
 // N tries to write CAN message
-#define NTRIES              (15)
+#define NTRIES              (3)
 
 // COB-ID base:
 #define NMT_COBID           0

commandline/cfg/2dirturret/Readme

@@ -0,0 +1,32 @@
+scripts for two-direction turret
+
+lambda/4
+rotates in negative direction
+speed=400
+full circle = 400 steps (12800 microsteps for 1/32)
+35.(5) microsteps per 1 degree
+
+turrret
+rotates in positive direction
+speed=1000
+full circle = 200 steps (6400 mircosteps for 1/32)
+six positions, so distance between positions is 1066.(6) microsteps
+
+
+scripts:
+
+Tinit - init stepper driver (run just after power on)
+Tgotopos - move turret to given position (0..5)
+Rgotoangle - rotate rotator to given angle (0..359 degrees)
+
+WARNING! If LIM1=1 (i.e. one of limit switches active) but lsw activated by other device than you want move,
+you won't be able to move it!
+
+So, if you need to move turret to some position when rotator is at 0 angle, you should first remove rotator from
+limit switch (at least to 30 degrees), then rotate turret and after all move rotator to given angle.
+The same is for turret position 2: it always on LIM1, so to move rotator when turret is on 2 you should move
+turret to any other position first, after that you will be able to rotate rotator
+
+To check just run 
+steppermove -vvvw -E3 -I11
+(twice, if first run won't show you limit switches), and you will see value of LIM1.

commandline/cfg/2dirturret/Rgotoangle

@@ -0,0 +1,46 @@
+#!/bin/bash
+
+CANID=11
+STPPERROT=12800
+
+function onlim(){
+	eval $(steppermove -I11 -E3 | grep LIM1)
+	echo $LIM1
+}
+
+
+if [[ $# != 1 ]]; then
+	echo -e "Usage: $(basename $0) position\n\twhere 'position' is an angle [integer number!!!] from 0 to 360\t0 - limit switch position" >&2
+	exit 1
+fi
+
+if (( $1 > 360 || $1 < 0 )); then
+	echo "'position' should be from 0 360" >&2
+	exit 1
+fi
+
+steppermove -I${CANID} -m1000 
+if [[ $(onlim) == "1" ]]; then
+	echo "Go out from limit switch"
+	steppermove -I${CANID} -cA
+	steppermove -I${CANID} -r -$((STPPERROT / 4)) -w
+	steppermove -I${CANID} -E3 -m1000
+	if (( $(onlim) == 1 )); then
+		echo "TURRET is on limit switch, rotate it first!" >&2
+		exit 1
+	fi
+fi
+
+echo "Go to limit switch"
+steppermove -I${CANID} -E3 -r -$STPPERROT -w
+steppermove -I${CANID} -cA -w
+
+if (( $1 != 0 )); then
+	STP=$(printf %.0f $(echo "12800/360*$1 + 31" | bc -l))
+	STP=$(( STP/32*32 ))
+	echo "Go to angle $1"
+	steppermove -I${CANID} -r -$STP -w
+fi
+
+echo "OK, at place!"
+steppermove -I${CANID} -E3 

commandline/cfg/2dirturret/Start_settings.cfg

@@ -0,0 +1,35 @@
+# Transmit SDO to driver
+# Format: index, subindex, data
+
+# Set heartbeat time to 0
+0x1017, 0, 0
+
+# Turn off offline programming
+0x6018, 1, 0
+0x6018, 2, 0
+
+# Set Node ID to 11
+0x2002, 0, 11
+# 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, 2
+# Set deceleration
+0x6009, 0, 2
+
+# EXT1&EXT2 emergency stop @ rising edge (normal state == 0)
+# Rising edge for triggers
+0x600F, 2, 3
+# Pullup for triggers on rising edge
+0x600F, 3, 3
+# Enable 
+0x600F, 1, 3
+
+
+# Save parameters
+0x2007, 0, 2

commandline/cfg/2dirturret/Tgotopos

@@ -0,0 +1,43 @@
+#!/bin/bash
+
+CANID=11
+STPPERROT=6400
+# positions: amount of steps for each turret position
+POSITIONS=(4224   5291   6357   7424   8491   9557)
+
+function onlim(){
+    eval $(steppermove -I11 -E3 | grep LIM1)
+    echo $LIM1
+}
+
+if [[ $# != 1 ]]; then
+	echo -e "Usage: $(basename $0) position\n\twhere 'position' is a number from 0 to 5\t0 is closed diaphragm" >&2
+	exit 1
+fi
+
+if (( $1 > 5 || $1 < 0 )); then
+	echo "'position' should be from 0 (closed) to 5" >&2
+	exit 1
+fi
+
+steppermove -I${CANID} -m400
+if [[ $(onlim) == 1 ]]; then
+    echo "Go out from limit switch"
+    steppermove -I${CANID} -cA
+    steppermove -I${CANID} -m1000 -r $((STPPERROT / 4)) -w
+    steppermove -I${CANID} -m400
+    if (( $(onlim) == 1 )); then
+        echo "ROTATOR is on limit switch, rotate it first!" >&2
+        exit 1
+    fi
+fi
+
+echo "Go to limit switch"
+steppermove -I${CANID} -E3 -r $STPPERROT -w
+steppermove -I${CANID} -cA -w
+
+echo "Go to position  $1"
+steppermove -I${CANID} -r ${POSITIONS[$1]} -w
+
+echo "OK, at place!"
+steppermove -I${CANID} -E3

commandline/cfg/2dirturret/Tinit

@@ -0,0 +1,5 @@
+#!/bin/sh
+
+CANID=11
+
+steppermove -vvv -I${CANID} -cE3 -s250 -m400 -w

commandline/cfg/3axiscorr/Readme

@@ -0,0 +1 @@
+scripts for 3-motor local corrector

commandline/cfg/3axiscorr/loccorr_init

@@ -0,0 +1,19 @@
+#!/bin/sh
+
+# init 3-point local corrector 
+
+for I in 2 10 11; do
+	./steppermove -I$I -m8000 -r-10000
+done
+
+for I in 2 10 11; do
+	./steppermove -I$I -w
+done
+
+for I in 2 10 11; do
+	./steppermove -I$I -0
+done
+
+for I in 2 10 11; do
+	./steppermove -I$I -a 7200
+done

commandline/cfg/3axiscorr/loccorr_telepalka

@@ -0,0 +1,24 @@
+#!/bin/sh
+
+# random move 3-point local corrector around middle point
+
+function rmove(){ # $1 - current motor index, $2 - index of motor to correct into+, $3 - relative steps
+	arr=(2 10 11) # ID's of motors
+	ID=${arr[$1]}
+	#./steppermove -I$ID -w
+	if [[ $1 -ne $2 ]]; then # move to minus
+		./steppermove -I$ID -a $(( 7200 - S ))
+	else # move to plus
+		./steppermove -I$ID -a $(( 7200 + S ))
+	fi
+}
+
+trap "echo Exited!; exit;" SIGINT SIGTERM
+
+while true; do
+	I=$(( RANDOM % 3 ))
+	S=$(( RANDOM % 1280 ))
+	for i in 0 1 2; do
+		rmove $i $I $S
+	done
+done

commandline/cfg/Start_settings.cfg

@@ -8,13 +8,19 @@
 0x6018, 1, 0
 0x6018, 2, 0
 
-# Set Node ID to 10
+# Set Node ID to 11
-0x2002, 0, 10
+0x2002, 0, 11
 # 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.5A
 0x600B, 0, 1500
+# Set acceleration (0 - no, 1 - highest, 8 - lowest)
+0x6008, 0, 2
+# Set deceleration
+0x6009, 0, 2
 
 # EXT1&EXT2 emergency stop: EXT1@falling edge (normal state == 1), EXT2@rising edge (normal state == 0)
 # Falling edge for trigger #1 

commandline/cfg/Start_settingsA.cfg

@@ -0,0 +1,20 @@
+# Transmit SDO to driver
+# Format: index, subindex, data
+
+# Set heartbeat time to 0
+0x1017, 0, 0
+
+# Turn off offline programming
+0x6018, 1, 0
+0x6018, 2, 0
+
+# Set Node ID 
+0x2002, 0, 11
+# 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 x mA
+0x600B, 0, 1000
+
+# Save parameters
+0x2007, 0, 2

commandline/cfg/endswitchesOFF.cfg

@@ -0,0 +1,2 @@
+# Disable EXT1, EXT2
+0x600F, 1, 0

commandline/cfg/endswitches_NC_Pullup.cfg

@@ -0,0 +1,13 @@
+# Transmit SDO to driver
+# Format: index, subindex, data
+
+# EXT1 emergency stop @ rising edge (normal state == 0)
+# Rising edge for trigger #1 
+0x600F, 2, 3
+# Pullup for triggers on rising edge
+0x600F, 3, 3
+# Enable EXT1
+0x600F, 1, 3
+
+# save settings
+#0x2007, 0, 2

commandline/dicentries.in

@@ -103,6 +103,8 @@ DICENTRY(POSITION,      0x600C, 0, 4, 1, "current position")
 DICENTRY(CURRREDUCT,    0x600D, 0, 1, 0, "current reduction")
 // motor enable
 DICENTRY(ENABLE,        0x600E, 0, 1, 0, "motor enable")
+// EXT emergency stop Npar
+DICENTRY(EXTNPAR,       0x600F, 0, 1, 0, "EXT emergency stop number of parameters")
 // EXT emergency stop enable
 DICENTRY(EXTENABLE,     0x600F, 1, 1, 0, "EXT emergency stop enable")
 // EXT emergency stop trigger mode
@@ -120,6 +122,8 @@ DICENTRY(STALLPARS,     0x6017, 0, 2, 0, "stall parameters (open loop)")
 // offline operation
 DICENTRY(OFFLNMBR,      0x6018, 1, 1, 0, "Number of offline programming command")
 DICENTRY(OFFLENBL,      0x6018, 2, 1, 0, "Offline automatic operation enable")
+// EXT stabilize delay
+DICENTRY(EXTSTABDELAY,  0x601A, 0, 2, 0, "EXT stabilize delay (ms)")
 // stall set
 DICENTRY(STALLSET,      0x601B, 0, 1, 0, "stall set (open loop)")
 // absolute displacement

commandline/main.c

@@ -123,7 +123,7 @@ static inline void gpioval(int64_t es){
     }
     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);
+        printf("LIM%d=%d\n", i, (EXTACTIVE(i, v)));
     }
 }
 
@@ -138,8 +138,8 @@ static inline void showAllPars(){
                   entry->index, entry->subindex, entry->name);
             continue;
         }
-        printf("# %s\n0x%04X, %d, %ld (0x%lX)\n", entry->name, entry->index,
+        printf("# %s\n0x%04X, %d, %ld (0x%X)\n", entry->name, entry->index,
-               entry->subindex, val, val);
+               entry->subindex, val, (uint32_t)val);
         fflush(stdout);
     }
     printf("\n\n");
@@ -171,7 +171,6 @@ static inline void wait_busy(){
 
 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
@@ -198,8 +197,7 @@ int main(int argc, char *argv[]){
     }
     if(GP->enableESW && GP->disableESW) ERRX("Enable & disable ESW can't meet together");
 
-    check4running(self, GP->pidfile);
+    check4running(NULL, GP->pidfile);
-    free(self);
     signal(SIGTERM, signals); // kill (-15) - quit
     signal(SIGHUP, SIG_IGN);  // hup - ignore
     signal(SIGINT, signals);  // ctrl+C - quit
@@ -236,6 +234,23 @@ int main(int argc, char *argv[]){
         getSDOe(MICROSTEPS, setusteps, "Can't get microstepping");
         Mesg("MICROSTEPS: %g\n", dtime() - d0);
     }*/
+    // enable limit switches
+    if(GP->enableESW){
+        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);
     if(!GP->quick){
         // check error status
         getSDOe(ERRSTATE, chkerr, "Can't get error status");
@@ -265,7 +280,7 @@ int main(int argc, char *argv[]){
         else message(2, "MICROSTEPS=%u", i64);
     }
     i64 = SDO_read(&ENCRESOL, ID);
-    if(i64 == INT64_MIN) LogAndWarn("Can't get encoder resolution value");
+    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
@@ -304,24 +319,6 @@ int main(int argc, char *argv[]){
             ++p;
         }
     }
-    // enable limit switches
-    if(GP->enableESW){
-        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);
-    // 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);
@@ -342,6 +339,7 @@ int main(int argc, char *argv[]){
             ERRX("Can't move to relative position %d", GP->relmove);
         Mesg("RelMove: %g\n", dtime() - d0);
     }
+    if(GP->wait) wait_busy();
 #undef getSDOe
 
 #if 0

commandline/pusirobot.h

@@ -46,7 +46,7 @@ extern const SDO_dic_entry* allrecords[];
 #define SPEED_MULTIPLIER    (1.6)
 
 // limit switches mask in GPIO status register (x=1,2,3)
-#define EXTMASK(x)      (1<<(6+x))
+#define EXTMASK(x)      (1<<(7+x))
 
 #define EXTACTIVE(x, reg)   ((reg&EXTMASK(x)) ? 1:0)