Change motion parameters and focus to positive direction

Z1000_focus/HW_dependent.h

@@ -45,24 +45,33 @@
 // rev/min to raw speed value
 #define RAWSPEED(x)         (x*5)
 // max/min speed (rev/min)
-#define MAXSPEED            1200
-#define MINSPEED            350
+#define MAXSPEED            (1200)
+#define MINSPEED            (350)
+// encoder differences (if larger) for speed (MAXSPEED, MAXSPEED/2 and MAXSPEED/3) select
+#define ENCODER_DIFF_SPEED1 (1500)
+#define ENCODER_DIFF_SPEED2 (500)
+#define ENCODER_DIFF_SPEED3 (150)
 // speed to move from ESW
-#define ESWSPEED            350
+#define ESWSPEED            (350)
 // moving timeout: 5minutes
 #define MOVING_TIMEOUT      (300)
+// correction parameters: steps after stopping = CORR0 + (CORR1 + CORR2*rs)*rs)
+// where rs is raw speed
+#define CORR0               (-46.0)
+#define CORR1               (4.2857e-3)
+#define CORR2               (1.5714e-5)
 
 
 // constants for focus conversion: foc_mm = (foc_raw - FOCRAW_0) / FOCSCALE_MM
-#define FOCSCALE_MM         4096.
-#define FOCRAW_0            15963187.
+#define FOCSCALE_MM         (4096.)
+#define FOCRAW_0            (15963187.)
 #define FOC_RAW2MM(x)       (((x)-FOCRAW_0)/FOCSCALE_MM)
 #define FOC_MM2RAW(x)       (FOCRAW_0+(x)*FOCSCALE_MM)
 
 // raw position precision - 2.5um
 #define RAWPOS_TOLERANCE    10
-// raw dF value for accurate focussing (rough move to F+dF0 and after this slow move to F)
-#define dF0                 500
+// raw dF value for accurate focussing (rough move to F-dF0 and after this slow move to F)
+#define dF0                 250
 // minimal & maximal focus positions (should be >min+dF0 & <max-dF0)
 #define FOCMIN_MM           2.75
 #define FOCMAX_MM           76.

Z1000_focus/Readme_calculate_PARAMS

@@ -0,0 +1,13 @@
+Запустить в режиме standalone с нужной скоростью:
+./can_focus -Am <speed>
+Смотреть последнюю строчку выхлопа: Stopped for <время остановки> <DPOS>
+DPOS здесь - количество шагов, на которое "уехал" фокус после команды "стоп".
+Погонять с разными скоростями в разных направлениях, посчитать примерную зависимость.
+
+Посчитать в октаве:
+speed=5*[400 600 800 1000 1200]' % скорость (скорость в шагах и rawspeed отличаются в 5 раз)
+dx=[25 110 220 370 545]' % усредненные между + и - смещения
+N=[ones(size(steps)) steps steps.^2] % матрица аргумента
+k = N \ dx % коэффициенты аппроксимации dx = k0 + k1*steps + k2*steps^2
+
+

Z1000_focus/can_encoder.c

@@ -706,7 +706,8 @@ static int move(unsigned long targposition, int16_t rawspeed){
     // in rawspeed = -27.96 + 1.84e-2*v + 1.52e-5*v^2
     double rs = fabs((double)rawspeed);
     //double cv = (-27.96 + (1.84e-2 + 1.52e-5*rs)*rs);
-    double cv = (-50. + (1.84e-2 + 1.52e-5*rs)*rs);
+    double cv = (CORR0 + (CORR1 + CORR2 * rs)*rs);
+    DBG("Corr coefficients: %g, %g, %g; raw speed: %g", CORR0, CORR1, CORR2, rs);
     long corrvalue = (long) cv; // correction due to stopping ramp
     if(corrvalue < 10) corrvalue = 10;
     DBG("start-> curpos: %ld, difference: %ld, corrval: %ld",
@@ -755,7 +756,7 @@ static int move(unsigned long targposition, int16_t rawspeed){
             ++errctr;
             DBG("errctr: %d", errctr);
             if(errctr > STALL_MAXCTR) break;
-        }
+        }else errctr = 0;
         olddiffr = diffr;
     }
     if(can_dtime() - t0 > MOVING_TIMEOUT){
@@ -796,13 +797,12 @@ int move2pos(double target){
         verbose("Already at position\n");
         return 0;
     }
-    long spd, targ0pos = (long)targposition + (long)dF0, absdiff = labs(targ0pos - (long)curposition),
+    long spd, targ0pos = (long)targposition - (long)dF0, absdiff = labs(targ0pos - (long)curposition),
             sign = (targ0pos > (long)curposition) ? 1 : -1;
     DBG("absdiff: %ld", absdiff);
-    // move not less than for 1s
-    if(absdiff > 1500) spd = sign*MAXSPEED;
-    else if(absdiff > 500) spd = sign*MAXSPEED / 2;
-    else if(absdiff > 150) spd = sign*MAXSPEED / 3;
+    if(absdiff > ENCODER_DIFF_SPEED1) spd = sign*MAXSPEED;
+    else if(absdiff > ENCODER_DIFF_SPEED2) spd = sign*MAXSPEED / 2;
+    else if(absdiff > ENCODER_DIFF_SPEED3) spd = sign*MAXSPEED / 3;
     else spd = sign*MINSPEED;
     int16_t targspd = (int16_t) spd;
     DBG("TARGSPD: %d", targspd);
@@ -810,16 +810,16 @@ int move2pos(double target){
     else if(spd < INT16_MIN) targspd = INT16_MIN;
     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
+    if(targposition > curposition){ // we are from the left
+        if(targspd > MINSPEED*3/2){ // omit rough moving to focus value if there's too little distance towards target
             // rough moving
-            DBG("1) ROUGH move to the LEFT: curpos=%ld, difference=%ld\n", curposition, targ0pos - (long)curposition);
+            DBG("1) ROUGH move to the RIGHT: curpos=%ld, difference=%ld\n", curposition, targ0pos - (long)curposition);
             if(move(targ0pos, RAWSPEED(targspd))){
                 return 1;
             }
         }
-    }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);
+    }else{ // we are from the right - move to the point @ left side of target
+        DBG("1) ROUGH move to the LEFT: curpos=%ld, difference=%ld\n", curposition, targ0pos - (long)curposition);
         if(move(targ0pos, RAWSPEED(targspd))){
             DBG("Error in move?");
             return 1;
@@ -835,17 +835,13 @@ int move2pos(double target){
         DBG("Catch the position @ rough moving");
         return 0;
     }
-    if(curposition < targposition){
+    if(curposition > targposition){ // we should be from the left of target
         WARNX("Error in current position: %.3f instead of %.3f!", FOC_RAW2MM(curposition), FOC_RAW2MM(targposition));
         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)))){
+    if(move(targposition, RAWSPEED(MINSPEED))){
         WARNX("Can't catch focus precisely!");
         return 1;
     }

Z1000_focus/main.c

@@ -45,8 +45,8 @@ int verbose(const char *fmt, ...){
     return i;
 }
 
-static pid_t childpid;
 void signals(int signo){
+    unlink_pidfile();
     can_exit(signo);
 }
 
@@ -89,20 +89,17 @@ int main (int argc, char *argv[]){
     signal(SIGTSTP, SIG_IGN);
     signal(SIGHUP, SIG_IGN);
     can_dev[8] = '1';
-/*
-    if(G->server || G->standalone){ // init hardware
-        check4running(G->pidfilename);
-    }
-*/
+
     if(G->server){ // daemonize & run server
-#if !defined EBUG
+#ifndef EBUG
         if(daemon(1, 0)){
             ERR("daemon()");
-        };
+        }
 #endif
         check4running(G->pidfilename);
+#ifndef EBUG
         while(1){ // guard for dead processes
-            childpid = fork();
+            pid_t childpid = fork();
             if(childpid){
                 DBG("child: %d", childpid);
                 wait(NULL);
@@ -121,6 +118,13 @@ int main (int argc, char *argv[]){
                 daemonize(G->port);
             }
         }
+#else
+    if(G->logname){ // open log file in child
+        openlogfile(G->logname);
+        putlog("Running in debug mode, PID %d", getpid());
+    }
+    daemonize(G->port);
+#endif
     }else if(!G->standalone){
         cmdparser();
         return 0;