From abe934338b0e942492805b6228df0063bebed36d Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Tue, 9 Mar 2021 19:18:24 +0300 Subject: [PATCH] Change motion parameters and focus to positive direction --- Z1000_focus/HW_dependent.h | 23 ++++++++++++++------- Z1000_focus/Readme_calculate_PARAMS | 13 ++++++++++++ Z1000_focus/can_encoder.c | 32 +++++++++++++---------------- Z1000_focus/main.c | 22 ++++++++++++-------- 4 files changed, 56 insertions(+), 34 deletions(-) create mode 100644 Z1000_focus/Readme_calculate_PARAMS diff --git a/Z1000_focus/HW_dependent.h b/Z1000_focus/HW_dependent.h index caf1402..949199a 100644 --- a/Z1000_focus/HW_dependent.h +++ b/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 & +Смотреть последнюю строчку выхлопа: Stopped for <время остановки> +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 + + diff --git a/Z1000_focus/can_encoder.c b/Z1000_focus/can_encoder.c index 57612c8..3840319 100644 --- a/Z1000_focus/can_encoder.c +++ b/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; } diff --git a/Z1000_focus/main.c b/Z1000_focus/main.c index 43b3b5b..73317bc 100644 --- a/Z1000_focus/main.c +++ b/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;