Seems that it's working!

Z1000_focus/HW_dependent.h

@@ -24,6 +24,7 @@
 
 // amount of encoder's counts when stop = rawspeed^2/STOPPING_COEFF
 #define STOPPING_COEFF      (117500L)
+//#define STOPPING_COEFF      (1175000L)
 
 // direction of motor rotation positive to encoder (1 - negative)
 #define MOTOR_REVERSE       (0)
@@ -48,26 +49,26 @@
 // moving timeout (*0.01s)
 #define MOVING_TIMEOUT      5000
 
+
+// constants for focus conversion: foc_mm = (foc_raw - FOCRAW_0) / FOCSCALE_MM
+#define FOCSCALE_MM         4096.
+#define FOCRAW_0            40960.
+#define FOC_RAW2MM(x)       (((x)-FOCRAW_0)/FOCSCALE_MM)
+#define FOC_MM2RAW(x)       (FOCRAW_0+(x)*FOCSCALE_MM)
+
 // raw position precision
-#define RAWPOS_TOLERANCE    20
+#define RAWPOS_TOLERANCE    10
 // raw dF value for accurate focussing
 #define dF0                 100
 // minimal & maximal focus positions (should be >min+dF0 & <max-dF0)
-#define FOCMIN              200
-#define FOCMAX              320000
-// focus raw positions @ endswitches (< or >)
-#define FOCPOS_CW_ESW       5000
-#define FOCPOS_CCW_ESW      315000
-// the same in mm
 #define FOCMIN_MM           0.1
 #define FOCMAX_MM           76.0
+// -//- raw values
+#define FOCMIN              FOC_MM2RAW(FOCMIN_MM)
+#define FOCMAX              FOC_MM2RAW(FOCMAX_MM)
 // permitted distance to end-switch
 #define ESW_DIST_ALLOW      1.0
-
+// focus raw positions @ endswitches (< or >)
-// constants for focus conversion: foc_mm = (foc_raw - FOCRAW_MM0) / FOCSCALE_MM
+#define FOCPOS_CW_ESW       FOC_MM2RAW((FOCMAX_MM - ESW_DIST_ALLOW))
-#define FOCSCALE_MM         4096.
+#define FOCPOS_CCW_ESW      FOC_MM2RAW((FOCMIN_MM + ESW_DIST_ALLOW))
-#define FOCRAW_MM0          0
-#define FOC_RAW2MM(x)       ((x-FOCRAW_MM0)/FOCSCALE_MM)
-#define FOC_MM2RAW(x)       (FOCRAW_MM0+x*FOCSCALE_MM)
-
 #endif // HW_DEPENDENT__

Z1000_focus/can_encoder.c

@@ -31,24 +31,31 @@ extern int verbose(const char *fmt, ...);
 
 // CAN bus IDs: for motor's functions (PI ID [F=4] == PO ID[F=3] + 1) and parameters
 static unsigned long motor_id = 0, motor_p_id = 0;//, bcast_id = 1;
-// current motor position
+// current motor position (RAW)
 static unsigned long curposition = 0;
 // encoder's node number
 static int encnodenum = 0;
 // system status
 static sysstatus curstatus = STAT_OK;
+// current raw motor speed (without MOTOR_REVERSE)
+int16_t targspd = 0;
+
 static canstatus can_write_par(uint8_t subidx, uint16_t idx, uint32_t *parval);
 static canstatus can_read_par(uint8_t subidx, uint16_t idx, uint32_t *parval);
 static int move(unsigned long targposition, int16_t rawspeed);
+static int waitTillStop();
 
+// check if end-switches are in default state
 // return 0 if all OK
 static int chk_eswstates(){
+    FNAME();
     uint32_t cw, ccw;
     if(CAN_NOERR != can_read_par(PAR_DI_SUBIDX, PAR_CW_IDX, &cw)) goto verybad;
     if(CAN_NOERR != can_read_par(PAR_DI_SUBIDX, PAR_CCW_IDX, &ccw)) goto verybad;
     uint32_t parval = DI_ENSTOP;
     if(cw != DI_ENSTOP || ccw != DI_ENSTOP){ // activate enable/stop
-        WARNX("\n\nThe end-switches state wasn't default!");
+        WARNX("\nThe end-switches state wasn't default!");
+        if(waitTillStop()) return 1; // we can change motor parameters only in stopped state
         if(CAN_NOERR != can_write_par(PAR_DI_SUBIDX, PAR_CW_IDX, &parval)) goto verybad;
         if(CAN_NOERR != can_write_par(PAR_DI_SUBIDX, PAR_CCW_IDX, &parval)) goto verybad;
     }
@@ -59,6 +66,50 @@ verybad:
     return 1;
 }
 
+/**
+ * @brief chkMove - check whether moving available
+ * @param spd - speed (used only its sign) before MOTOR_REVERSE!
+ * @return 0 if all OK
+ */
+static int chkMove(int spd){
+    //FNAME();
+    if(curstatus == STAT_DAMAGE){
+        WARNX("Try to move in damaged state");
+        return 1;
+    }
+    eswstate e;
+    if(CAN_NOERR != get_endswitches(&e)){
+        curstatus = STAT_ERROR;
+        return 1;
+    }
+    if(e == ESW_INACTIVE){
+        if(getPos(NULL)) return 1;
+        if(curposition <= FOCMIN && spd < 0){
+            WARNX("Try to move to the left of minimal position");
+            return 1;
+        }
+        if(curposition >= FOCMAX && spd > 0){
+            WARNX("Try to move to the right of maximal position");
+            return 1;
+        }
+        return 0;
+    }
+    if(e == ESW_BOTH_ACTIVE){
+        curstatus = STAT_DAMAGE;
+        return 1;
+    }
+    if(e == ESW_CCW_ACTIVE && spd < 0){
+        curstatus = STAT_ESW;
+        WARNX("Try to move over the CCW end-switch");
+        return 1;
+    }else if(e == ESW_CW_ACTIVE && spd > 0){
+        curstatus = STAT_ESW;
+        WARNX("Try to move over the CW end-switch");
+        return 1;
+    }
+    return 0;
+}
+
 /**
  * @brief init_encoder - encoder's interface initialisation
  * @param encnode - encoder's node number
@@ -135,32 +186,41 @@ int init_encoder(int encnode, int reset){
  * @return 0 if all OK
  */
 int go_out_from_ESW(){
-    uint32_t cw, ccw, parval;
+    FNAME();
-    // check esw roles
+    uint32_t parval;
-    if(CAN_NOERR != can_read_par(PAR_DI_SUBIDX, PAR_CW_IDX, &cw)) goto bad;
+    if(chk_eswstates()) return 1;
-    if(CAN_NOERR != can_read_par(PAR_DI_SUBIDX, PAR_CCW_IDX, &ccw)) goto bad;
-    parval = DI_ENSTOP;
-    if(cw != DI_ENSTOP || ccw != DI_ENSTOP){ // activate enable/stop
-        if(CAN_NOERR != can_write_par(PAR_DI_SUBIDX, PAR_CW_IDX, &parval)) goto bad;
-        if(CAN_NOERR != can_write_par(PAR_DI_SUBIDX, PAR_CCW_IDX, &parval)) goto bad;
-    }
     // check esw
     eswstate e;
+    getPos(NULL);
     if(CAN_NOERR != get_endswitches(&e)){
-        WARNX("Can't read end-switches state");
         return 1;
     }
     if(e == ESW_BOTH_ACTIVE){ // error situation!
         WARNX("Error: both end-switches are active!");
-        curstatus = STAT_BOTHESW;
+        curstatus = STAT_DAMAGE;
         return 1;
     }
     if(e == ESW_INACTIVE){
         DBG("Esw inactive");
+        int r = 0;
+        if(curposition < FOCMIN) r = move(FOCMIN, MAXSPEED);
+        else if(curposition > FOCMAX) r = move(FOCMAX, -MAXSPEED);
         curstatus = STAT_OK;
-        return 0;
+        return r;
     }
     curstatus = STAT_GOFROMESW;
+    // check that current position is in available zone
+    if(e == ESW_CW_ACTIVE && curposition < FOCPOS_CW_ESW){
+        // WTF? CW end-switch activated in forbidden zone!
+        WARNX("CW end-switch in forbidden zone (to the left of normal position)!");
+        curstatus = STAT_DAMAGE;
+        return 1;
+    }else if(e == ESW_CCW_ACTIVE && curposition > FOCPOS_CW_ESW){
+        // CCW end-switch activated in forbidden zone!
+        WARNX("CCW end-switch in forbidden zone (too far)!");
+        curstatus = STAT_DAMAGE;
+        return 1;
+    }
     // try to move from esw
     parval = DI_NOFUNC;
     uint16_t idx = (e == ESW_CW_ACTIVE) ? PAR_CW_IDX : PAR_CCW_IDX;
@@ -170,23 +230,17 @@ int go_out_from_ESW(){
         getPos(NULL);
         DBG("try %d, pos: %lu, E=%d", i, curposition, e);
         unsigned long targ = (e == ESW_CW_ACTIVE) ? curposition - (double)FOCSCALE_MM*0.2 : curposition + (double)FOCSCALE_MM*0.2;
+        if(targ > FOCMAX) targ = FOCMAX;
+        else if(targ < FOCMIN) targ = FOCMIN;
         if(move(targ, speed)) continue;
         get_endswitches(&e);
         if(e == ESW_INACTIVE) break;
     }
-    // return esw state
+    if(chk_eswstates()) return 1;
-    parval = DI_ENSTOP;
-    if(CAN_NOERR != can_write_par(PAR_DI_SUBIDX, PAR_CW_IDX,  &parval)) goto bad;
-    if(CAN_NOERR != can_write_par(PAR_DI_SUBIDX, PAR_CCW_IDX, &parval)) goto bad;
-    if(e != ESW_INACTIVE){
-        WARNX("Can't move out of end-switch");
-        curstatus = STAT_ERROR;
-        return 1;
-    }
     curstatus = STAT_OK;
     return 0;
 bad:
-    WARNX("Can't get/set esw parameters");
+    WARNX("Can't move out from end-switch");
     curstatus = STAT_ERROR;
     return 1;
 }
@@ -216,28 +270,39 @@ int init_motor_ids(int addr){
  * @return 0 if all OK
  */
 int getPos(double *pos){
+    //FNAME();
     int r = !(getLong(encnodenum, DS406_POSITION_VAL, 0, &curposition));
     if(pos) *pos = FOC_RAW2MM(curposition);
     eswstate e;
     if(CAN_NOERR != get_endswitches(&e)){
-        WARNX("Can't read end-switches state");
         curstatus = STAT_ERROR;
     }else switch(e){
         case ESW_BOTH_ACTIVE:
-            curstatus = STAT_BOTHESW;
+            curstatus = STAT_DAMAGE;
         break;
         case ESW_CCW_ACTIVE:
-            if(curposition > FOCPOS_CCW_ESW) curstatus = STAT_BADESW;
+            if(curposition > FOCPOS_CCW_ESW) curstatus = STAT_DAMAGE;
             else curstatus = STAT_ESW;
         break;
         case ESW_CW_ACTIVE:
-            if(curposition < FOCPOS_CW_ESW) curstatus = STAT_BADESW;
+            if(curposition < FOCPOS_CW_ESW) curstatus = STAT_DAMAGE;
             else curstatus = STAT_ESW;
         break;
         case ESW_INACTIVE:
         default:
             curstatus = STAT_OK;
     }
+    if(targspd){
+        if(curposition <= FOCMIN && targspd < 0){ // bad value
+            WARNX("Forbidden position < FOCMIN!");
+            stop();
+            curstatus = STAT_FORBIDDEN;
+        }else if(curposition >= FOCMAX && targspd > 0){
+            WARNX("Forbidden position > FOCMAX!");
+            stop();
+            curstatus = STAT_FORBIDDEN;
+        }
+    }
     return r;
 }
 
@@ -262,8 +327,8 @@ void returnPreOper(){
 }
 
 /**
- * @brief fix_targspeed - fix raw speed value if it is greater MAXSPEED or less than MINSPEED
+ * @brief fix_targspeed - fix speed value if it is greater MAXSPEED or less than MINSPEED
- * @param targspd (io) - raw target speed
+ * @param targspd (io) - target speed in rev/min
  */
 static void fix_targspeed(int16_t *targspd){
     if(!targspd) return;
@@ -302,7 +367,7 @@ static canstatus can_send_chk(unsigned char *buf, unsigned char *obuf){
         can_dsleep(0.01);
     }
     if(I == 50){
-        WARNX("Error getting answer");
+        WARNX("can_send_chk(): error getting answer");
         return CAN_NOANSWER;
     }
     if(obuf) memcpy(obuf, rdata, l);
@@ -336,25 +401,26 @@ static canstatus can_send_param(unsigned char *buf, unsigned char *obuf){
     int I, rxpnt, idr, dlen;
     double rxtime;
     unsigned char rdata[8];
-    can_clean_recv(&rxpnt, &rxtime);
+/*
-    if(can_send_frame(motor_p_id, l, buf) <= 0){
-        WARNX("Error sending CAN frame (len %d)", l);
-        return CAN_CANTSEND;
-    }
-    /*
 green("Sent param:     ");
 for(int i=0; i<l; ++i) printf("0x%02x ", buf[i]);
 printf("\n");
 */
+    if(can_send_frame(motor_p_id, l, buf) <= 0){
+        WARNX("Error sending CAN frame (len %d)", l);
+        return CAN_CANTSEND;
+    }
+    can_clean_recv(&rxpnt, &rxtime);
     for(I = 0; I < 50; ++I){
         if(can_recv_frame(&rxpnt, &rxtime, &idr, &dlen, rdata) && (idr&0x1fffffff) == motor_p_id+1) break;
+        //DBG("Got frame from ID 0x%x", idr&0x1fffffff);
         can_dsleep(0.01);
     }
     if(I == 50){
-        WARNX("Error getting answer");
+        WARNX("can_send_param(): error getting answer");
         return CAN_NOANSWER;
     }
-    /*
+/*
 green("Received param: ");
 for(int i=0; i<dlen; ++i) printf("0x%02x ", rdata[i]);
 printf("\n");
@@ -402,15 +468,15 @@ static canstatus can_write_par(uint8_t subidx, uint16_t idx, uint32_t *parval){
     buf[3] = idx & 0xff;
     if(parval){
         uint32_t par = *parval;
-        obuf[4] = (par >> 24) & 0xff;
+        DBG("parameter: %d", par);
-        obuf[5] = (par >> 16) & 0xff;
+        buf[4] = (par >> 24) & 0xff;
-        obuf[6] = (par >> 8)  & 0xff;
+        buf[5] = (par >> 16) & 0xff;
-        obuf[7] = par & 0xff;
+        buf[6] = (par >> 8)  & 0xff;
+        buf[7] = par & 0xff;
     }
     return can_send_param(buf, obuf);
 }
 
-
 /**
  * @brief get_motor_speed
  * @param motstatus (o) - status (if !NULL)
@@ -427,14 +493,6 @@ canstatus get_motor_speed(double *spd){
         return s;
     }
     *spd = (double)speed.i / 1000.;
-    /*
-    union{
-        int16_t i16;
-        uint8_t c8[2];
-    } mspd;
-    mspd.c8[0] = rdata[3];
-    mspd.c8[1] = rdata[2];
-    */
     return CAN_NOERR;
 }
 
@@ -444,8 +502,13 @@ canstatus get_motor_speed(double *spd){
  * @return
  */
 canstatus get_endswitches(eswstate *Esw){
+    //FNAME();
     uint32_t val = 0;
     canstatus s = can_read_par(PAR_DI_SUBIDX, PAR_DIST_IDX, &val);
+    if(s != CAN_NOERR){
+        WARNX("Can't read end-switches state");
+        return s;
+    }
     if(Esw){
         int v = 0;
         if(!(val & ESW_CW)){ // + pressed
@@ -464,27 +527,68 @@ canstatus get_endswitches(eswstate *Esw){
  * @return 0 if all OK
  */
 int stop(){
+    FNAME();
     unsigned char buf[6] = {0, CW_STOP,0,};
     if(can_send_chk(buf, NULL)){
         WARNX("Can't stop motor!");
         return 1;
     }
+    targspd = 0;
+    return 0;
+}
+
+/**
+ * @brief waitTillStop - wait for full stop
+ */
+static int waitTillStop(){
+    long oldposition = -1;
+    double spd;
+    int r = 0;
+    if(CAN_NOERR == get_motor_speed(&spd)){
+        DBG("speed: %g, targspd: %d", spd, targspd);
+        if(fabs(spd) > DBL_EPSILON || targspd) r = stop();
+    }else r = stop();
+    if(r) r = stop();
+    if(!r){
+        do{ // wait till stop
+            can_dsleep(0.1);
+            if(CAN_NOERR == get_motor_speed(&spd)){
+                if(fabs(spd) > DBL_EPSILON){
+                    //DBG("Still moving, spd=%g", spd);
+                    //stop();
+                    continue; // wait for zero-speed
+                }else{
+                    DBG("OK, stopped, spd=%g", spd);
+                }
+            }else{DBG("can't get motor speed");};
+            oldposition = curposition;
+            // now wait for full moving stop
+            getLong(encnodenum, DS406_POSITION_VAL, 0, &curposition);
+            //DBG("curpos: %lu, oldpos: %ld", curposition, oldposition);
+        }while((long)curposition != oldposition);
+    }else{
+        curstatus = STAT_ERROR;
+        return 1;
+    }
     return 0;
 }
 
 /**
  * @brief movewconstspeed - move with constant speed
- * @param rawspd - given speed
+ * @param spd - given speed (rev/min)
  * @return 0 if all OK
  */
-int movewconstspeed(int spd){
+int movewconstspeed(int16_t spd){
-    if(chk_eswstates()) return 1;
+    if(chkMove(spd)) return 1;
-    int16_t targspd = RAWSPEED(spd);
+    fix_targspeed(&spd);
+    targspd = RAWSPEED(spd);
     unsigned char buf[8] = {0,};
     buf[1] = CW_ENABLE;
-    if(MOTOR_REVERSE) spd = -spd;
+    int16_t s = targspd;
-    buf[2] = (targspd >> 8) & 0xff;
+    if(MOTOR_REVERSE) s = -s;
-    buf[3] = targspd & 0xff;
+    buf[2] = (s >> 8) & 0xff;
+    buf[3] = s & 0xff;
+    DBG("\tBUF: %d, %d, %d, %d", buf[0], buf[1], buf[2], buf[3]);
     if(can_send_chk(buf, NULL)){
         WARNX("Can't move motor!");
         return 1;
@@ -495,27 +599,32 @@ int movewconstspeed(int spd){
 /**
  * @brief move - move focuser from current position to approximately `targposition` with speed `rawspeed`
  * @param targposition - target position in raw value
- * @param rawspeed - speed in raw value
+ * @param rawspeed - raw speed value
  * @return 0 if all OK
  */
 static int move(unsigned long targposition, int16_t rawspeed){
+    //FNAME();
     if(abs(targposition - curposition) < RAWPOS_TOLERANCE){
         verbose("Already at position\n");
+        DBG("Already at position");
         return 0;
     }
-    if(chk_eswstates()) return 1;
+    if(chkMove(rawspeed)) return 1;
     unsigned char buf[6] = {0,};
-    DBG("Start moving with speed %d", rawspeed);
+    DBG("Start moving with speed %d, target position: %lu", rawspeed, targposition);
     buf[1] = CW_ENABLE;
+    targspd = rawspeed;
     if(MOTOR_REVERSE) rawspeed = -rawspeed;
     buf[2] = (rawspeed >> 8) & 0xff;
     buf[3] = rawspeed & 0xff;
     DBG("\tBUF: %d, %d, %d, %d", buf[0], buf[1], buf[2], buf[3]);
+ //unsigned char obuf[8];
     if(can_send_chk(buf, NULL)){
         WARNX("Can't move motor!");
         stop();
         return 1;
     }
+    //DBG("\tOBUF: %d, %d, %d, %d, %d, %d", obuf[0], obuf[1], obuf[2], obuf[3], obuf[4], obuf[5]);
 #ifdef EBUG
     double t0 = can_dtime();
 #endif
@@ -523,9 +632,9 @@ static int move(unsigned long targposition, int16_t rawspeed){
     long corrvalue = (long)rawspeed*(long)rawspeed / STOPPING_COEFF; // correction due to stopping ramp
     DBG("start-> curpos: %ld, difference: %ld, corrval: %ld, tm=%g",
         curposition, olddiffr, corrvalue, can_dtime()-t0);
-    int i, zerctr = 0;
+    int i, zerctr = 0, errctr = 0;
     for(i = 0; i < MOVING_TIMEOUT; ++i){
-        can_dsleep(0.01);
+        can_dsleep(0.001);
         //uint16_t motstat;
         double speed;
         if(emerg_stop){ // emergency stop activated
@@ -541,42 +650,11 @@ static int move(unsigned long targposition, int16_t rawspeed){
             return 1;
         }
         getPos(NULL);
-        if(curstatus != STAT_OK){
+        if(chkMove(targspd)){
-            WARNX("Something bad with end-switches");
+            WARNX("Can't move further!");
             stop();
             return 1;
         }
-        /* eswstate esw;
-        if(get_endswitches(&esw) != CAN_NOERR){
-            WARNX("Can't get endswitches state, stopping");
-            stop();
-            curstatus = STAT_ERROR;
-            return 1;
-        }
-        if(esw != ESW_INACTIVE){ // OOps, something wrong
-            if(esw == ESW_BOTH_ACTIVE){ // error!
-                WARNX("Check end-switches, both active!");
-                curstatus = STAT_BOTHESW;
-                stop();
-                return 1;
-            }
-//TODO: check wrong end-switches!
-            if(rawspeed > 0){ // moving CW, don't care for CCW esw state
-                if(esw == ESW_CW_ACTIVE){
-                    WARNX("CW mowing: end-switch!");
-                    curstatus = STAT_ESW;
-                    stop();
-                    return 1;
-                }
-            }else{ // movig CCW
-                if(esw == ESW_CCW_ACTIVE){
-                    WARNX("CCW mowing: end-switch!");
-                    curstatus = STAT_ESW;
-                    stop();
-                    return 1;
-                }
-            }
-        }*/
         if(fabs(speed) < 0.1){ // || (motstat & (SW_B_UNBLOCK|SW_B_READY|SW_B_POUNBLOCK)) != (SW_B_UNBLOCK|SW_B_READY|SW_B_POUNBLOCK)){
             if(++zerctr == 10){
                 WARNX("Motor stopped while moving!");
@@ -587,12 +665,17 @@ static int move(unsigned long targposition, int16_t rawspeed){
         }else zerctr = 0;
         if(!getLong(encnodenum, DS406_POSITION_VAL, 0, &curposition)) continue;
         long diffr = targposition - curposition;
-        //DBG("Speed: %g, curpos: %ld, diff: %ld", speed, curposition, diffr);
+    DBG("Speed: %g, curpos: %ld, diff: %ld", speed, curposition, diffr);
-        if(abs(diffr) < corrvalue || abs(diffr) - RAWPOS_TOLERANCE > abs(olddiffr)){
+        if(abs(diffr) < corrvalue || abs(diffr) < RAWPOS_TOLERANCE){
             DBG("OK! almost reach: olddif=%ld, diff=%ld, corrval=%ld, tm=%g", olddiffr, diffr, corrvalue, can_dtime()-t0);
             olddiffr = diffr;
             break;
         }
+        if(abs(diffr) > abs(olddiffr)){
+            ++errctr;
+            DBG("errctr: %d", errctr);
+            if(errctr > 50) break;
+        }
         olddiffr = diffr;
     }
     if(i == MOVING_TIMEOUT){
@@ -602,20 +685,12 @@ static int move(unsigned long targposition, int16_t rawspeed){
         return 1;
     }
     DBG("end-> curpos: %ld, difference: %ld, tm=%g\n", curposition, targposition - curposition, can_dtime()-t0);
-    long oldposition;
+    if(waitTillStop()) return 1;
-    int r = stop();
-    if(r) r = stop();
-    if(!r) do{ // wait till stop
-        oldposition = curposition;
-        can_dsleep(0.1);
-        getLong(encnodenum, DS406_POSITION_VAL, 0, &curposition);
-        //DBG("wait-> curpos: %ld, difference: %ld, tm=%g\n", curposition, targposition - curposition, can_dtime()-t0);
-    }while((long)curposition != oldposition);
     if(abs(targposition - curposition) > RAWPOS_TOLERANCE)
         verbose("Current (%ld) position is too far from target (%ld)\n", curposition, targposition);
     DBG("stop-> curpos: %ld, difference: %ld, tm=%g\n", curposition, targposition - curposition, can_dtime()-t0);
     curstatus = STAT_OK;
-    return r;
+    return 0;
 }
 
 /**
@@ -624,6 +699,7 @@ static int move(unsigned long targposition, int16_t rawspeed){
  * @return 0 if all OK
  */
 int move2pos(double target){
+    FNAME();
     double cur;
     if(getPos(&cur)){
         WARNX("Can't get current position!");
@@ -639,12 +715,18 @@ int move2pos(double target){
         verbose("Already at position\n");
         return 0;
     }
-    long targ0pos = (long)targposition + (long)dF0;
+    long spd, targ0pos = (long)targposition + (long)dF0, absdiff = abs(targ0pos - (long)curposition),
-    long spd = (targ0pos - (long)curposition) / 2L;
+            sign = (targ0pos > (long)curposition) ? 1 : -1;
+    DBG("absdiff: %ld", absdiff);
+    //long spd = (targ0pos - (long)curposition) / 2L;
+    if(absdiff > 1000) spd = sign*MAXSPEED;
+    else if(absdiff > 500) spd = sign*MAXSPEED / 2;
+    else if(absdiff > 200) spd = sign*MAXSPEED / 4;
+    else spd = sign*MINSPEED;
     int16_t targspd = (int16_t) spd;
-    if(spd > INT16_MAX) targspd = INT16_MAX;
+/*    if(spd > INT16_MAX) targspd = INT16_MAX;
     else if(spd < INT16_MIN) targspd = INT16_MIN;
-    fix_targspeed(&targspd);
+    fix_targspeed(&targspd);*/
     // check moving direction: thin focus correction always should run to negative!
     if(targposition < curposition){ // we are from the right
         if(targspd < -MINSPEED*3/2){ // omit rough moving to focus value if there's too little distance towards target
@@ -657,6 +739,7 @@ int move2pos(double target){
     }else{ // we are from the left - move to the point @ right side of target
         DBG("1) ROUGH move to the RIGHT: curpos=%ld, difference=%ld\n", curposition, targ0pos - (long)curposition);
         if(move(targ0pos, RAWSPEED(targspd))){
+            DBG("Error in move?");
             return 1;
         }
     }
@@ -667,6 +750,7 @@ int move2pos(double target){
     }
     if(abs(targposition - curposition) < RAWPOS_TOLERANCE){
         verbose("Catch the position @ rough movint\n");
+        DBG("Catch the position @ rough movint");
         return 0;
     }
     if(curposition < targposition){
@@ -674,6 +758,10 @@ int move2pos(double target){
         return 1;
     }
     DBG("2) curpos: %ld, difference: %ld\n", curposition, (long)targposition - (long)curposition);
+    //sleep(3);
+    /*DBG("NOW MOVE");
+    move(targposition, -800);
+    DBG("NOW MOVE MORE");*/
     // now make an accurate moving
     if(move(targposition, -(RAWSPEED(MINSPEED)))){
         WARNX("Can't catch focus precisely!");
@@ -687,11 +775,12 @@ int move2pos(double target){
         verbose("Stopped over the accuracy range\n");
         return 1;
     }
-    return stop();
+    return 0;
 }
 
 // return 0 if all OK
-static int get_pos_speed(unsigned long *pos, double *speed){
+int get_pos_speed(unsigned long *pos, double *speed){
+    FNAME();
     int ret = 0;
     if(pos){
         if(!getLong(encnodenum, DS406_POSITION_VAL, 0, pos)) ret = 1;

Z1000_focus/can_encoder.h

@@ -43,10 +43,10 @@ typedef enum{
     STAT_OK,        // all OK
     // blocking statuses:
     STAT_ESW,       // end-switch active
-    STAT_BADESW,    // wrong end-switch active
-    STAT_BOTHESW,   // both end-switches active
     STAT_GOFROMESW, // mowing from end-switch
-    STAT_ERROR      // uncoverable error
+    STAT_ERROR,     // error state
+    STAT_FORBIDDEN, // forbidden position
+    STAT_DAMAGE     // the device in damaged state and can't work further
 } sysstatus;
 
 int init_encoder(int encnode, int reset);
@@ -59,8 +59,9 @@ canstatus get_motor_speed(double *spd);
 canstatus get_endswitches(eswstate *Esw);
 int move2pos(double target);
 int stop();
-int movewconstspeed(int spd);
+int movewconstspeed(int16_t spd);
 int go_out_from_ESW();
 sysstatus get_status();
+int get_pos_speed(unsigned long *pos, double *speed);
 
 #endif // CAN_ENCODER_H__

Z1000_focus/checkfile.c

@@ -0,0 +1,135 @@
+/*
+ * daemon.c - functions for running in background like a daemon
+ *
+ * Copyright 2013 Edward V. Emelianoff <eddy@sao.ru>
+ *
+ * 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 2 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+ * MA 02110-1301, USA.
+ */
+
+#include <unistd.h>     // getpid, unlink
+#include <dirent.h>     // opendir
+#include "checkfile.h"
+#include "usefull_macros.h"
+
+/**
+ * @brief readPSname - read process name from /proc/PID/cmdline
+ * @param pid - PID of interesting process
+ * @return filename or NULL if not found
+ *      don't use this function twice for different names without copying
+ *      its returning by strdup, because `name` contains in static array
+ */
+char *readPSname(pid_t pid){
+    static char name[PATH_MAX];
+    char *pp = name, byte, path[PATH_MAX];
+    FILE *file;
+    int cntr = 0;
+    size_t sz;
+    snprintf(path, PATH_MAX, PROC_BASE "/%d/cmdline", pid);
+    file = fopen(path, "r");
+    if(!file) return NULL; // there's no such file
+    do{ // read basename
+        sz = fread(&byte, 1, 1, file);
+        if(sz != 1) break;
+        if(byte != '/') *pp++ = byte;
+        else{
+            pp = name;
+            cntr = 0;
+        }
+    }while(byte && cntr++ < PATH_MAX-1);
+    name[cntr] = 0;
+    fclose(file);
+    return name;
+}
+
+/**
+ * @brief iffound_default - default action when running process found
+ * @param pid - another process' pid
+ *      Redefine this function for user action
+ */
+void WEAK iffound_default(pid_t pid){
+    /// \nïÂÎÁÒÕÖÅÎ ÏÄÎÏÉÍÅÎÎÙÊ ÐÒÏÃÅÓÓ (pid=%d), ×ÙÈÏÄ.\n
+    ERRX("\nFound running process (pid=%d), exit.\n", pid);
+}
+
+static char *pidfilename_ = NULL; // store the name of pidfile here
+
+/**
+ * check wether there is a same running process
+ * exit if there is a running process or error
+ * Checking have 3 steps:
+ *      1) lock executable file
+ *      2) check pidfile (if you run a copy?)
+ *      3) check /proc for executables with the same name (no/wrong pidfile)
+ * @param pidfilename - name of pidfile or NULL if none
+ */
+void check4running(char *pidfilename){
+    DIR *dir;
+    FILE *pidfile;
+    struct dirent *de;
+    struct stat s_buf;
+    pid_t pid = 0, self;
+    char *name, *myname;
+    self = getpid(); // get self PID
+    if(!(dir = opendir(PROC_BASE))){ // open /proc directory
+        ERR(PROC_BASE);
+    }
+    if(!(name = readPSname(self))){ // error reading self name
+        ERR("Can't read self name");
+    }
+    myname = strdup(name);
+    if(pidfilename && stat(pidfilename, &s_buf) == 0){ // pidfile exists
+        pidfile = fopen(pidfilename, "r");
+        if(pidfile){
+            if(fscanf(pidfile, "%d", &pid) > 0){ // read PID of (possibly) running process
+                if((name = readPSname(pid)) && strncmp(name, myname, 255) == 0){
+                    iffound_default(pid);
+                    fclose(pidfile);
+                    free(myname);
+                    closedir(dir);
+                    return;
+                }
+            }
+            fclose(pidfile);
+        }
+    }
+    // There is no pidfile or it consists a wrong record
+    while((de = readdir(dir))){ // scan /proc
+        if(!(pid = (pid_t)atoi(de->d_name)) || pid == self) // pass non-PID files and self
+            continue;
+        if((name = readPSname(pid)) && strncmp(name, myname, 255) == 0)
+            iffound_default(pid);
+    }
+    closedir(dir);
+    free(myname);
+    // OK, not found -> create pid-file if need
+    if(pidfilename){
+        pidfile = fopen(pidfilename, "w");
+        /// îÅ ÍÏÇÕ ÏÔËÒÙÔØ PID ÆÁÊÌ
+        if(!pidfile) ERR("Can't open PID file");
+        fprintf(pidfile, "%d\n", self); // write self PID to pidfile
+        fclose(pidfile);
+        pidfilename_ = strdup(pidfilename);
+    }
+}
+
+/**
+ * @brief unlink_pidfile - remove pidfile @ exit
+ */
+void unlink_pidfile(){
+    if(!pidfilename_) return;
+    unlink(pidfilename_);
+    FREE(pidfilename_);
+}

Z1000_focus/checkfile.h

@@ -0,0 +1,39 @@
+/*
+ * This file is part of the Zphocus project.
+ * Copyright 2019 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 CHECKFILE_H__
+#define CHECKFILE_H__
+
+#ifndef PROC_BASE
+#define PROC_BASE "/proc"
+#endif
+#ifndef WEAK
+#define WEAK        __attribute__ ((weak))
+#endif
+
+// default function to run if another process found
+void WEAK iffound_default(pid_t pid);
+// check that our process is exclusive
+void check4running(char *pidfilename);
+// read name of process by its PID
+char *readPSname(pid_t pid);
+void unlink_pidfile();
+
+
+#endif // CHECKFILE_H__

Z1000_focus/cmdlnopts.c

@@ -33,13 +33,16 @@
 int help;
 glob_pars  G;
 
+#define DEFPIDNAME "/tmp/z1000focus.pid"
+
 //            DEFAULTS
 // default global parameters
 glob_pars const Gdefault = {
     .nodenum = 3,
     .motorID = 12,
     .gotopos = NAN,
-    .port = DEFPORT
+    .port = DEFPORT,
+    .pidfilename = DEFPIDNAME
 };
 
 /*
@@ -53,7 +56,6 @@ myoption cmdlnopts[] = {
     {"reset",   NO_ARGS,    NULL,   'r',    arg_none,   APTR(&G.reset),     "reset encoder"},
     {"verbose", NO_ARGS,    NULL,   'v',    arg_int,    APTR(&G.verbose),   "show more info"},
     {"motorid", NEED_ARG,   NULL,   'i',    arg_int,    APTR(&G.motorID),   "motor controller address"},
-    //{"bcastid", NEED_ARG,   NULL,   'b',    arg_int,    APTR(&G.motorID),   "motor controller broadcast address"},
     {"gotopos", NEED_ARG,   NULL,   'g',    arg_double, APTR(&G.gotopos),   "target focus position"},
     {"targspeed",NEED_ARG,  NULL,   't',    arg_double, APTR(&G.targspeed), "move motor with constant speed (rev/min)"},
     {"stop",    NO_ARGS,    NULL,   's',    arg_none,   APTR(&G.stop),      "stop motor"},
@@ -61,9 +63,10 @@ myoption cmdlnopts[] = {
     {"eswstate",NO_ARGS,    NULL,   'e',    arg_none,   APTR(&G.showesw),   "show end-switches state"},
     {"logfile", NEED_ARG,   NULL,   'l',    arg_string, APTR(&G.logname),   "logfile name and path"},
     {"server",  NO_ARGS,    NULL,   'S',    arg_none,   APTR(&G.server),    "work as server"},
-    {"port",    NEED_ARG,   NULL,   'p',    arg_string, APTR(&G.port),      "server port number"},
+    {"port",    NEED_ARG,   NULL,   'P',    arg_string, APTR(&G.port),      "server port number (default: " DEFPORT ")"},
     {"host",    NEED_ARG,   NULL,   'H',    arg_string, APTR(&G.host),      "host to connect (default: localhost)"},
     {"standalone",NO_ARGS,  NULL,   'A',    arg_none,   APTR(&G.standalone),"run as standalone application"},
+    {"pidfile", NEED_ARG,   NULL,   'p',    arg_string, APTR(&G.pidfilename),"name of PID-file (default: " DEFPIDNAME ")"},
     end_option
 };
 

Z1000_focus/cmdlnopts.h

@@ -31,7 +31,6 @@ typedef struct{
     int reset;              // reset encoder
     int verbose;            // more messages
     int motorID;            // motor address (from controller's settings)
-    int bcastID;            // broadcast motor address
     double gotopos;         // move focus to given position
     double targspeed;       // just rotate motor with given speed
     int stop;               // stop motor
@@ -42,6 +41,7 @@ typedef struct{
     char *port;             // port number for server or client
     char *host;             // host to connect (in client mode)
     int standalone;         // run standalone
+    char *pidfilename;      // name of PID-file
 } glob_pars;
 
 

Z1000_focus/main.c

@@ -22,6 +22,7 @@
 #include <signal.h>
 #include "can_encoder.h"
 #include "canopen.h"
+#include "checkfile.h"
 #include "cmdlnopts.h"
 #include "HW_dependent.h"
 #include "socket.h"
@@ -49,6 +50,7 @@ int verbose(const char *fmt, ...){
  */
 void signals(int signo){
     WARNX("Received signal %d", signo);
+    unlink_pidfile();
     exit(signo);
 }
 
@@ -87,26 +89,27 @@ int main (int argc, char *argv[]){
     }
 
     signal(SIGTERM, signals);
+    signal(SIGKILL, signals);
     signal(SIGTSTP, SIG_IGN);
     signal(SIGHUP, SIG_IGN);
 //can_dev[8] = '1';
 
     if(G->server || G->standalone){ // init hardware
+        check4running(G->pidfilename);
         if(G->logname){
             openlogfile(G->logname);
         }
         if(init_encoder(G->nodenum, G->reset)) ERRX("Encoder not found");
-
+        if(init_motor_ids(G->motorID)){
+            WARNX("Error during motor initialization");
+            ret = 1;
+            goto Oldcond;
+        }
         if(getPos(&curposition)){
             WARNX("Can't read current position");
             ret = 1;
             goto Oldcond;
         }else verbose("Position @ start: %.2fmm\n", curposition);
-
-        if(init_motor_ids(G->motorID)){
-            ret = 1;
-            goto Oldcond;
-        }
     }
 
     if(G->server){ // daemonize & run server
@@ -157,17 +160,20 @@ int main (int argc, char *argv[]){
         ret = move2pos(G->gotopos);
         goto Oldcond;
     }
+    double spd;
+    unsigned long pos;
 
 Oldcond:
-    if(getPos(&curposition)) WARNX("Can't read current position");
+    if(get_pos_speed(&pos, &spd)) WARNX("Can't read current position");
     else{
-        if(G->verbose) printf("pos=%.2fmm, ", curposition);
+        curposition = FOC_RAW2MM(pos);
-        else printf("%.2f\n", curposition);
+        verbose("speed=%d\n", spd);
+        if(G->verbose) printf("pos=%.03fmm, ", curposition);
+        else printf("%.03f\n", curposition);
     }
     if(G->showesw){
         eswstate e;
-        if(CAN_NOERR != get_endswitches(&e)) WARNX("Can't read end-switches state");
+        if(CAN_NOERR == get_endswitches(&e)) switch(e){
-        else switch(e){
             case ESW_INACTIVE:
                 green("End-switches inactive\n");
             break;
@@ -182,10 +188,6 @@ Oldcond:
                 red("ERROR: both end-switches active\n");
         }
     }
-    double spd;
-    if(get_motor_speed(&spd) == CAN_NOERR) verbose("speed=%d\n", spd);
-    else WARNX("Can't read speed");
-
     returnPreOper();
     return ret;
 }

Z1000_focus/motor_cancodes.h

@@ -78,18 +78,25 @@
 // Some parameters: indexes & subindexes
 // Digital inputs
 #define PAR_DI_SUBIDX   0
-// inputs state (lowest bit is DI00)
+// inputs state (lowest bit is DI00), 0x208E
 #define PAR_DIST_IDX    8334
 // Speed & current
 #define PAR_SPD_SUBIDX  0
 #define PAR_CRNT_SUBIDX 0
+// 0x207E - speed
 #define PAR_SPD_IDX     8318
+// 0x2086 - current
 #define PAR_CRNT_IDX    8326
 // inputs role
+// 0x228c - DI0
 #define PAR_DI00_IDX    8844
+// 0x2090
 #define PAR_DI02_IDX    8336
+// 0x2091
 #define PAR_DI03_IDX    8337
+// 0x2092
 #define PAR_DI04_IDX    8338
+// 0x2093
 #define PAR_DI05_IDX    8339
 // roles:
 #define DI_NOFUNC       0

Z1000_focus/socket.c

@@ -86,7 +86,7 @@ bool emerg_stop = FALSE;
 static int send_data(int sock, int webquery, char *buf){
     if(!buf) return 0;
     ssize_t L, Len = strlen(buf);
-    DBG("buf: %s, Len: %zd", buf, Len);
+    //DBG("buf: %s, Len: %zd", buf, Len);
     if(Len < 1) return 0;
     char tbuf[BUFLEN];
     // OK buffer ready, prepare to send it
@@ -160,6 +160,30 @@ static const char *startmoving(double pos){
     return S_ANS_OK;
 }
 
+/**
+ * @brief ego, getoutESW - run go_out_from_ESW in a separate thread
+ */
+static void *ego(_U_ void *unused){
+    DBG("MOVE OUT FROM END-SWITCH");
+    pthread_mutex_lock(&canbus_mutex);
+    go_out_from_ESW();
+    pthread_mutex_unlock(&canbus_mutex);
+    pthread_mutex_unlock(&moving_mutex);
+    pthread_exit(NULL);
+    return NULL;
+}
+static void getoutESW(){
+    pthread_mutex_lock(&moving_mutex);
+    pthread_t m_thread;
+    if(pthread_create(&m_thread, NULL, ego, NULL)){
+        WARN("pthread_create()");
+        pthread_mutex_unlock(&moving_mutex);
+    }else{
+        DBG("Thread created, detouch");
+        pthread_detach(m_thread); // don't care about thread state
+    }
+}
+
 static void *handle_socket(void *asock){
 #define getparam(x)     (strncmp(found, x, sizeof(x)-1) == 0)
     //putlog("handle_socket(): getpid: %d, pthread_self: %lu, tid: %lu",getpid(), pthread_self(), syscall(SYS_gettid));
@@ -170,14 +194,14 @@ static void *handle_socket(void *asock){
     double t0 = dtime();
     while(dtime() - t0 < SOCKET_TIMEOUT){
         if(!waittoread(sock)){ // no data incoming
-            DBG("no incoming data");
+            //DBG("no incoming data");
             continue;
         }
         if((rd = read(sock, buff, BUFLEN-1)) < 1){
-            DBG("socket closed. Exit");
+            //DBG("socket closed. Exit");
             break;
         }
-        DBG("Got %zd bytes", rd);
+        //DBG("Got %zd bytes", rd);
         // add trailing zero to be on the safe side
         buff[rd] = 0;
         // now we should check what do user want
@@ -189,10 +213,10 @@ static void *handle_socket(void *asock){
             // web query have format GET /some.resource
         }
         // here we can process user data
-        DBG("user send: %s%s\n", buff, webquery ? ", web" : "");
+        //DBG("user send: %s%s\n", buff, webquery ? ", web" : "");
         // empty request == focus request
         if(strlen(found) < 1 || getparam(S_CMD_FOCUS)){
-            DBG("position request");
+            //DBG("position request");
             snprintf(buff, BUFLEN, "%.03f", curPos());
         }else if(getparam(S_CMD_STOP)){
             DBG("Stop request");
@@ -232,11 +256,8 @@ static void *handle_socket(void *asock){
                 case STAT_OK:
                     msg = S_STATUS_OK;
                 break;
-                case STAT_BADESW:
+                case STAT_DAMAGE:
-                    msg = S_STATUS_BADESW;
+                    msg = S_STATUS_DAMAGE;
-                break;
-                case STAT_BOTHESW:
-                    msg = S_STATUS_BOTHESW;
                 break;
                 case STAT_ERROR:
                     msg = S_STATUS_ERROR;
@@ -247,6 +268,11 @@ static void *handle_socket(void *asock){
                 case STAT_GOFROMESW:
                     msg = S_STATUS_GOFROMESW;
                 break;
+                case STAT_FORBIDDEN:
+                    msg = S_STATUS_FORBIDDEN;
+                break;
+                default:
+                    msg = "Unknown status";
             }
             sprintf(buff, msg);
         }else sprintf(buff, S_ANS_ERR);
@@ -255,7 +281,7 @@ static void *handle_socket(void *asock){
         }
     }
     close(sock);
-    DBG("closed");
+    //DBG("closed");
     //putlog("socket closed, exit");
     pthread_exit(NULL);
     return NULL;
@@ -286,14 +312,13 @@ static void *server(void *asock){
         struct in_addr ipAddr = pV4Addr->sin_addr;
         char str[INET_ADDRSTRLEN];
         inet_ntop(AF_INET, &ipAddr, str, INET_ADDRSTRLEN);
-        //putlog("get connection from %s", str);
+        //DBG("Got connection from %s", str);
-        DBG("Got connection from %s", str);
         pthread_t handler_thread;
         if(pthread_create(&handler_thread, NULL, handle_socket, (void*) &newsock)){
             putlog("server(): pthread_create() failed");
             WARN("pthread_create()");
         }else{
-            DBG("Thread created, detouch");
+            //DBG("Thread created, detouch");
             pthread_detach(handler_thread); // don't care about thread state
         }
     }
@@ -322,13 +347,11 @@ static void daemon_(int sock){
         // get current position
         if(!pthread_mutex_trylock(&canbus_mutex)){
             getPos(NULL);
-            if(get_status() != STAT_OK){
+            sysstatus st = get_status();
-                if(!pthread_mutex_trylock(&moving_mutex)){
-                    go_out_from_ESW();
-                    pthread_mutex_unlock(&moving_mutex);
-                }
-            }
             pthread_mutex_unlock(&canbus_mutex);
+            if(st != STAT_OK){
+                getoutESW();
+            }
         }
     }while(1);
     putlog("daemon_(): UNREACHABLE CODE REACHED!");

Z1000_focus/socket.h

@@ -46,11 +46,11 @@
 // statuses
 #define S_STATUS_OK         "OK"
 #define S_STATUS_ESW        "End-switch active"
-#define S_STATUS_ERROR      "Uncoverable error"
+#define S_STATUS_ERROR      "Erroneous state"
-#define S_STATUS_BOTHESW    "Both end-switches active"
-#define S_STATUS_BADESW     "Wrong end-switch active"
 #define S_STATUS_ESW        "End-switch active"
 #define S_STATUS_GOFROMESW  "Moving from end-switch"
+#define S_STATUS_FORBIDDEN  "Motion in forbidden position"
+#define S_STATUS_DAMAGE     "The focuser damaged and can't work further"
 
 bool emerg_stop;