/* * This file is part of the loccorr project. * Copyright 2024 Edward V. Emelianov . * * 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 . */ #define _GNU_SOURCE #include #include #include #include #include #include #include #include #include #include #include "config.h" #include "debug.h" #include "improc.h" // global variable stopwork #include "steppers.h" #include "socket.h" // buffer for socket #define BUFLEN (256) // max time to wait answer "OK" from server #define WAITANSTIME (0.3) // amount of consequent center coordinates coincidence in `process_targetstate` #define NCONSEQ (2) // tolerance of coordinates coincidence (pix) #define COORDTOLERANCE (0.5) // PID typedef struct { double Kp, Ki, Kd; // coefficients double integral; // intergal error accumulator double prev_error; // previous error value for D double prev_time; // and previous time } PIDController; typedef enum{ STP_DISCONN, STP_RELAX, STP_SETUP, STP_GOTOTHEMIDDLE, STP_FINDTARGET, STP_FIX, STP_UNDEFINED, STP_STATE_AMOUNT } STPstate; typedef enum{ SETUP_NONE, // no setup SETUP_INIT, // the starting - move U&V to 0 SETUP_WAITUV0, // wait & move U&V to middle SETUP_WAITUVMID, // wait SETUP_WAITU0, // move U->0 SETUP_WAITUMAX, // move U->max SETUP_WAITV0, // V->0 SETUP_WAITVMAX, // V->max SETUP_FINISH } setupstatus; static _Atomic setupstatus sstatus = SETUP_NONE; // setup state //static int errctr = 0; // sending messages error counter (if > MAX_ERR_CTR, set state to disconnected) // common global variable for using in several sources (should be inited first) steppersproc *theSteppers = NULL; // stepper numbers typedef enum{ Ustepper = 0, Vstepper = 1, Fstepper = 2, } stepperno; const char *motornames[NMOTORS] = { [Ustepper] = "Umotor", [Vstepper] = "Vmotor", [Fstepper] = "Fmotor", }; // a list of steppers commands typedef enum{ CMD_ABSPOS, CMD_EMSTOP, CMD_ESW, CMD_GOTO, CMD_GOTOZ, CMD_RELPOS, CMD_STATE, CMD_STOP, CMD_AMOUNT } steppercmd; typedef enum{ ERR_OK, // 0 - all OK ERR_BADPAR, // 1 - parameter's value is wrong ERR_BADVAL, // 2 - wrong parameter's value ERR_WRONGLEN, // 3 - wrong message length ERR_BADCMD, // 4 - unknown command ERR_CANTRUN, // 5 - can't run given command due to bad parameters or other ERR_AMOUNT // amount of error codes } errcodes; static const char *stp_commands[CMD_AMOUNT] = { [CMD_ABSPOS] = "abspos", [CMD_EMSTOP] = "emstop", [CMD_ESW] = "esw", [CMD_GOTO] = "goto", [CMD_GOTOZ] = "gotoz", [CMD_RELPOS] = "relpos", [CMD_STATE] = "state", [CMD_STOP] = "stop", }; static const char* errtxt[ERR_AMOUNT] = { [ERR_OK] = "OK", [ERR_BADPAR] = "BADPAR", [ERR_BADVAL] = "BADVAL", [ERR_WRONGLEN] = "WRONGLEN", [ERR_BADCMD] = "BADCMD", [ERR_CANTRUN] = "CANTRUN", }; static atomic_int LastErr = ERR_OK; static sl_sock_t *serialsock = NULL; static pthread_t clientthread; static STPstate state = STP_DISCONN; // server state // this flag set to TRUE when next Xc,Yc available static volatile atomic_bool coordsRdy = FALSE; static double Xtarget = 0., Ytarget = 0.; // Values to change U,V and F by hands // flag & new focus value static volatile atomic_bool chfocus = FALSE; static volatile atomic_int newfocpos = 0, dUmove = 0, dVmove = 0; static volatile atomic_bool motorsoff = FALSE; // flag to disconnect // current steps counters (zero at the middle) static volatile atomic_int motposition[NMOTORS] = {0}; // relative position change after current moving ends (from external command) static volatile atomic_int motrelsteps[NMOTORS] = {0}; // current motor state static volatile atomic_int motstates[NMOTORS] = {0}; static uint8_t fixerr = 0; // ==1 if can't fixed // motor states: typedef enum{ STATE_RELAX, STATE_ACCEL, STATE_MOVE, STATE_MVSLOW, STATE_DECEL, STATE_STALL, STATE_ERR, STATE_NUM } motstate; static const char *str_states[STATE_NUM] = { [STATE_RELAX] = "relax", [STATE_ACCEL] = "accelerated", [STATE_MOVE] = "moving", [STATE_MVSLOW] = "slow moving", [STATE_DECEL] = "decelerated", [STATE_STALL] = "stalled", [STATE_ERR] = "error", }; TRUE_INLINE int relaxed(int nmotor){return motstates[nmotor] == STATE_RELAX;} #define Uposition (motposition[Ustepper]) #define Vposition (motposition[Vstepper]) #define Fposition (motposition[Fstepper]) static int nth_motor_setter(steppercmd idx, int n, int p); static void stp_disc(){ motorsoff = TRUE; } static void stp_disconnect(){ DBG("Try to disconnect"); LOGDBG("Try to disconnect"); if(serialsock){ DBG("Close socket"); sl_sock_delete(&serialsock); if(!pthread_cancel(clientthread)){ DBG("Join canceled thread"); pthread_join(clientthread, NULL); DBG("OK"); } LOGWARN("Stepper server disconnected"); } state = STP_DISCONN; } // check if nmot is U/V/F and return FALSE if not static int chkNmot(int nmot){ if(nmot == Ustepper || nmot == Vstepper || nmot == Fstepper) return TRUE; return FALSE; } static errcodes parser(steppercmd idx, int nmot, int ival){ int goodidx = chkNmot(nmot); switch(idx){ case CMD_ABSPOS: if(goodidx) motposition[nmot] = ival; else return ERR_BADPAR; break; case CMD_RELPOS: if(goodidx) motrelsteps[nmot] = ival; else return ERR_BADPAR; break; case CMD_STATE: if(!goodidx) return ERR_BADPAR; motstates[nmot] = ival; if(chkNmot(nmot)){ // one of our motors - check err or stall if(ival == STATE_STALL || ival == STATE_ERR){ WARNX("BAD status of motor %d", nmot); LOGWARN("BAD status of motor %d", nmot); nth_motor_setter(CMD_EMSTOP, nmot, 1); // tty to clear error } } break; case CMD_EMSTOP: case CMD_ESW: case CMD_GOTO: case CMD_GOTOZ: case CMD_STOP: break; default: return ERR_BADCMD; } return ERR_OK; } // check if message is error text static errcodes getecode(const char *msg){ errcodes e; for(e = 0; e < ERR_AMOUNT; ++e){ if(0 == strcmp(msg, errtxt[e])) break; } DBG("ERRcode: %d, (%s)", e, (e != ERR_AMOUNT) ? errtxt[e] : "undef"); LOGDBG("ERRcode: %d, (%s)", e, (e != ERR_AMOUNT) ? errtxt[e] : "undef"); return e; } /** * @brief parse_msg - parse last message * @param msg - message received */ static void parse_msg(char *msg){ char value[128]; if(!msg) return; char *key = get_keyval(msg, value); if(key){ int ival = atoi(value); //LOGDBG("key = %s, value = %s (%d)", key, value, ival); size_t l = strlen(key); size_t numpos = strcspn(key, "0123456789"); int parno = -1; if(numpos < l){ parno = atoi(key + numpos); key[numpos] = 0; } //DBG("numpos=%zd, parno=%d", numpos, parno); if(parno > -1){ // got motor number if(!chkNmot(parno)){ DBG("Not our business"); free(key); return; } } for(int idx = 0; idx < CMD_AMOUNT; ++idx){ // search index in commands if(0 == strcmp(stp_commands[idx], key)){ // found our //LOGDBG("OK, idx=%d, cmd=%s", idx, stp_commands[idx]); LastErr = parser(idx, parno, ival); break; } } free(key); return; }else{ DBG("GOT NON-setter %s", msg); errcodes e = getecode(msg); if(e != ERR_AMOUNT) LastErr = e; // for checking } } /** * @brief send_message - send character string `msg` to serial server, get and parse answer * @param msg - message (for setters could be like "N=M" or "=M") or NULL (for getters) * @return FALSE if failed (should reconnect) */ static errcodes send_message(steppercmd idx, char *msg){ // FNAME(); if(!serialsock){ WARNX("Not connected to serial socket!"); return ERR_CANTRUN; } char buf[256]; size_t msglen; if(!msg) msglen = snprintf(buf, 255, "%s\n", stp_commands[idx]); else msglen = snprintf(buf, 255, "%s%s\n", stp_commands[idx], msg); //DBG("Send message '%s', len %zd", buf, msglen); LastErr = ERR_AMOUNT; if(sl_sock_sendstrmessage(serialsock, buf) != (ssize_t)msglen){ WARN("send()"); LOGWARN("send_message(): send() failed"); return ERR_WRONGLEN; } //LOGDBG("send_message(): message '%s' sent", buf); double t0 = sl_dtime(); while(sl_dtime() - t0 < STEPPERS_NOANS_TIMEOUT){ if(LastErr != ERR_AMOUNT) return LastErr; } LOGWARN("send_message(): got NO answer for %s%s", stp_commands[idx], msg); return ERR_CANTRUN; } // send command cmd to n'th motor with param p, @return FALSE if failed static int nth_motor_setter(steppercmd idx, int n, int p){ if(idx < 0 || idx >= CMD_AMOUNT) return FALSE; char buf[256]; if(n < 0){ // setter without number snprintf(buf, 255, "=%d", p); DBG("nth_motor_setter(): set %s=%d", stp_commands[idx], p); LOGDBG("nth_motor_setter(): set %s=%d", stp_commands[idx], p); }else if(n < NMOTORS){ snprintf(buf, 255, "%d=%d", n, p); DBG("nth_motor_setter(): get %s%d=%d", stp_commands[idx], n, p); LOGDBG("nth_motor_setter(): set %s%d=%d", stp_commands[idx], n, p); }else{ WARNX("Wrong motno %d", n); LOGWARN("Wrong motno %d (cmd=%s, setter=%d)", n, stp_commands[idx], p); } if(ERR_OK != send_message(idx, buf)) return FALSE; return TRUE; } // and simplest getter static int nth_motor_getter(steppercmd idx, int n){ if(idx < 0 || idx >= CMD_AMOUNT) return FALSE; char buf[32], *msg = NULL; if(n > -1 && n < NMOTORS){ sprintf(buf, "%d", n); //DBG("nth_motor_getter(): %s%d", stp_commands[idx], n); //LOGDBG("nth_motor_getter(): %s%d", stp_commands[idx], n); msg = buf; }else{ WARNX("Wrong motno %d", n); LOGWARN("nth_motor_getter(): wrong motno %d (cmd=%s)", n, stp_commands[idx]); } if(ERR_OK != send_message(idx, msg)) return FALSE; return TRUE; } // send getter to all motors; return FALSE if failed static int chkmots(steppercmd cmd){ if( nth_motor_getter(cmd, Ustepper) && nth_motor_getter(cmd, Vstepper) && nth_motor_getter(cmd, Fstepper)) return TRUE; return FALSE; } static void chkall(){ chkmots(CMD_STATE); chkmots(CMD_ABSPOS); chkmots(CMD_RELPOS); } /** * @brief clientproc - process data received from serial terminal * @param par - socket * @return NULL */ void *clientproc(void _U_ *par){ FNAME(); char rbuf[BUFSIZ]; if(!serialsock) return NULL; double t0 = sl_dtime(); do{ ssize_t got = sl_sock_readline(serialsock, rbuf, BUFSIZ); //LOGDBG("got=%zd", got); if(got < 0 || sl_dtime() - t0 > STEPPERS_NOANS_TIMEOUT){ // disconnected WARNX("Serial server disconnected"); LOGERR("Serial server disconnected (timeout reached)"); if(serialsock) sl_sock_delete(&serialsock); state = STP_DISCONN; return NULL; }else if(got == 0){ // nothing to read from serial port usleep(1000); continue; } //LOGDBG("clientproc(): got '%s'", rbuf); // process data parse_msg(rbuf); t0 = sl_dtime(); } while(serialsock && serialsock->connected); WARNX("disconnected"); if(serialsock) sl_sock_delete(&serialsock); return NULL; } /** * @brief stp_connect_server - try connect to a local steppers serial server * @return FALSE if failed */ static int stp_connect_server(){ FNAME(); stp_disconnect(); char node[32]; snprintf(node, 31, "%d", theconf.stpserverport); DBG("Try to connect via port %s", node); serialsock = sl_sock_run_client(SOCKT_NETLOCAL, node, 4096); if(!serialsock){ DBG("Can't connect to serial server via port %s", node); return FALSE; } if(pthread_create(&clientthread, NULL, clientproc, NULL)){ sl_sock_delete(&serialsock); LOGWARN("stp_connect_server(): can't create client thread"); return FALSE; } // register and set max speed; don't check `register` answer as they could be registered already state = STP_RELAX; sstatus = SETUP_NONE; LOGMSG("Connected to stepper server"); return TRUE; } static void *stp_process_states(_U_ void *arg); static pthread_t processingthread; static void process_movetomiddle_stage(){ switch(sstatus){ case SETUP_INIT: // initial moving if( !nth_motor_setter(CMD_EMSTOP, Ustepper, 1) || !nth_motor_setter(CMD_EMSTOP, Vstepper, 1) || !nth_motor_setter(CMD_EMSTOP, Fstepper, 1) ) break; if(nth_motor_setter(CMD_GOTOZ, Ustepper, 1) && nth_motor_setter(CMD_GOTOZ, Vstepper, 1) && nth_motor_setter(CMD_GOTOZ, Fstepper, 1)){ LOGMSG("process_movetomiddle_stage(): SETUP_WAITUV0"); sstatus = SETUP_WAITUV0; } break; case SETUP_WAITUV0: // wait for all coordinates moving to zero if(!relaxed(Ustepper) || !relaxed(Vstepper) || !relaxed(Fstepper)) break; // didn't reach yet // now all motors are stopped -> send positions to zero if( !nth_motor_setter(CMD_ABSPOS, Ustepper, 0) || !nth_motor_setter(CMD_ABSPOS, Vstepper, 0) || !nth_motor_setter(CMD_ABSPOS, Fstepper, 0)) break; DBG("Reached UVF0!"); // goto if(nth_motor_setter(CMD_GOTO, Ustepper, (theconf.maxUpos + theconf.minUpos)/2) && nth_motor_setter(CMD_GOTO, Vstepper, (theconf.maxVpos + theconf.minVpos)/2) && nth_motor_setter(CMD_GOTO, Fstepper, (theconf.maxFpos + theconf.minFpos)/2)){ LOGMSG("process_movetomiddle_stage(): SETUP_WAITUVMID"); sstatus = SETUP_WAITUVMID; } break; case SETUP_WAITUVMID: // wait for the middle if(!relaxed(Ustepper) || !relaxed(Vstepper) || !relaxed(Fstepper)) break; // if motors ready, relsteps should be 0 if(motrelsteps[Ustepper] || motrelsteps[Vstepper] || motrelsteps[Fstepper]){ WARNX("Come to wrong pos: U=%d, V=%d, F=%d", Uposition, Vposition, Fposition); LOGWARN("Come to wrong pos: U=%d, V=%d, F=%d", Uposition, Vposition, Fposition); sstatus = SETUP_WAITUV0; } DBG("Reached middle position"); LOGMSG("Reached middle position"); // fallthrough default: sstatus = SETUP_NONE; state = STP_RELAX; } } /** * @brief process_setup_stage - process all stages of axes setup */ static void process_setup_stage(){ DBG("PROCESS: %d\n", sstatus); // coordinates for corrections calculation static double X0U, Y0U, XmU, YmU; static double X0V, Y0V, XmV, YmV; switch(sstatus){ case SETUP_INIT: // initial moving; don't move F (as it should be focused already) if( !nth_motor_setter(CMD_EMSTOP, Ustepper, 1) || !nth_motor_setter(CMD_EMSTOP, Vstepper, 1) ) break; if(nth_motor_setter(CMD_GOTOZ, Ustepper, 1) && nth_motor_setter(CMD_GOTOZ, Vstepper, 1) ){ LOGMSG("process_setup_stage(): SETUP_WAITUV0"); sstatus = SETUP_WAITUV0; } break; case SETUP_WAITUV0: // wait for both coordinates moving to zero if(!relaxed(Ustepper) || !relaxed(Vstepper)) break; // set current position to 0 if( !nth_motor_setter(CMD_ABSPOS, Ustepper, 0) || !nth_motor_setter(CMD_ABSPOS, Vstepper, 0)) break; DBG("ZERO border reached"); // goto middle if(nth_motor_setter(CMD_GOTO, Ustepper, (theconf.maxUpos+theconf.minUpos)/2) && nth_motor_setter(CMD_GOTO, Vstepper, (theconf.maxVpos+theconf.minVpos)/2)){ LOGMSG("process_setup_stage(): SETUP_WAITUVMID"); sstatus = SETUP_WAITUVMID; }else{ WARNX("Can't move U/V to middle"); LOGWARN("Can't move U/V to middle"); sstatus = SETUP_INIT; } break; case SETUP_WAITUVMID: // wait for the middle if(!relaxed(Ustepper) || !relaxed(Vstepper)) break; DBG("The middle reached"); // now move U to zero if(nth_motor_setter(CMD_GOTO, Ustepper, theconf.minUpos)){ LOGMSG("process_setup_stage(): SETUP_WAITU0"); sstatus = SETUP_WAITU0; }else{ LOGWARN("Can't move U to min"); sstatus = SETUP_INIT; } break; case SETUP_WAITU0: // wait while U moves to zero if(!coordsRdy) return; coordsRdy = FALSE; X0U = Xtarget; Y0U = Ytarget; DBG("got X0U=%.1f, Y0U=%.1f", X0U, Y0U); LOGDBG("got X0U=%.1f, Y0U=%.1f", X0U, Y0U); // move U to max if(nth_motor_setter(CMD_GOTO, Ustepper, theconf.maxUpos)){ LOGMSG("process_setup_stage(): SETUP_WAITUMAX"); sstatus = SETUP_WAITUMAX; }else{ LOGWARN("Can't move U to max"); sstatus = SETUP_INIT; } break; case SETUP_WAITUMAX: // wait while U moves to UVworkrange if(!coordsRdy) return; coordsRdy = FALSE; XmU = Xtarget; YmU = Ytarget; LOGDBG("got XmU=%.1f, YmU=%.1f", XmU, YmU); // now move U to zero and V to min if(nth_motor_setter(CMD_GOTO, Ustepper, (theconf.maxUpos+theconf.minUpos)/2) && nth_motor_setter(CMD_GOTO, Vstepper, theconf.minVpos)){ LOGMSG("process_setup_stage(): SETUP_WAITV0"); sstatus = SETUP_WAITV0; }else{ LOGWARN("Can't move U -> mid OR/AND V -> min"); sstatus = SETUP_INIT; } break; case SETUP_WAITV0: // wait while V moves to 0 if(!coordsRdy) return; coordsRdy = FALSE; X0V = Xtarget; Y0V = Ytarget; LOGDBG("got X0V=%.1f, Y0V=%.1f", X0V, Y0V); if(nth_motor_setter(CMD_GOTO, Vstepper, theconf.maxVpos)){ LOGMSG("process_setup_stage(): SETUP_WAITVMAX"); sstatus = SETUP_WAITVMAX; }else{ LOGWARN("Can't move V -> max"); sstatus = SETUP_INIT; } break; case SETUP_WAITVMAX: // wait while V moves to UVworkrange if(!coordsRdy) return; coordsRdy = FALSE; XmV = Xtarget; YmV = Ytarget; LOGDBG("got XmV=%.1f, YmV=%.1f", XmV, YmV); // calculate double dxU = XmU - X0U, dyU = YmU - Y0U, dxV = XmV - X0V, dyV = YmV - Y0V; LOGDBG("dxU=%.1f, dyU=%.1f, dxV=%.1f, dyV=%.1f", dxU, dyU, dxV, dyV); double sqU = sqrt(dxU*dxU + dyU*dyU), sqV = sqrt(dxV*dxV + dyV*dyV); LOGDBG("sqU=%g, sqV=%g", sqU, sqV); if(sqU < DBL_EPSILON || sqV < DBL_EPSILON) goto endmoving; // TODO: check configuration !!111111 // proportion coefficients for axes double KU = (theconf.maxUpos - theconf.minUpos) / sqU; double KV = (theconf.maxVpos - theconf.minVpos) / sqV; double sa = dyU/sqU, ca = dxU/sqU, sb = dyV/sqV, cb = dxV/sqV; // sin(alpha) etc LOGDBG("KU=%.4f, KV=%.4f, sa=%.4f, ca=%.4f, sb=%.4f, cb=%.4f", KU, KV, sa, ca, sb, cb); /* * [dX dY] = M*[dU dV], M = [ca/KU cb/KV; sa/KU sb/KV] ===> * [dU dV] = inv(M)*[dX dY], * inv(M) = 1/(ca/KU*sb/KV - sa/KU*cb/KV)*[sb/KV -cb/KV; -sa/KU ca/KU] */ double mul = 1. / (ca/KU*sb/KV - sa/KU*cb/KV); theconf.Kxu = mul*sb/KV; theconf.Kyu = -mul*cb/KV; theconf.Kxv = -mul*sa/KU; theconf.Kyv = mul*ca/KU; LOGMSG("process_setup_stage(): Kxu=%g, Kyu=%g; Kxv=%g, Kyv=%g", theconf.Kxu, theconf.Kyu, theconf.Kxv, theconf.Kyv); DBG("Now save new configuration"); saveconf(NULL); // try to store configuration // fallthrough endmoving: if(nth_motor_setter(CMD_GOTO, Vstepper, (theconf.maxVpos+theconf.minVpos)/2)) sstatus = SETUP_FINISH; break; case SETUP_FINISH: // goto middle again if(!relaxed(Ustepper) || !relaxed(Vstepper)) break; sstatus = SETUP_NONE; state = STP_RELAX; break; default: // SETUP_NONE - do nothing return; } } // process target finding stage (target should be fixed for at least NCONSEQ frames within COORDTOLERANCE) // return TRUE if finished static int process_targetstage(double X, double Y){ static double xprev = 0., yprev = 0.; static int nhit = 0; if(fabs(X - xprev) > COORDTOLERANCE || fabs(Y - yprev) > COORDTOLERANCE){ DBG("tolerance too bad: dx=%g, dy=%g", X-xprev, Y-yprev); nhit = 0; xprev = X; yprev = Y; return FALSE; }else if(++nhit < NCONSEQ){ DBG("nhit = %d", nhit); return FALSE; } theconf.xtarget = X + theconf.xoff; theconf.ytarget = Y + theconf.yoff; DBG("Got target coordinates: (%.1f, %.1f)", X, Y); LOGMSG("Got target coordinates: (%.1f, %.1f)", X, Y); saveconf(NULL); nhit = 0; xprev = 0.; yprev = 0.; return TRUE; } /** * @brief compute_pid - calculate PID responce for error * @param pid - U/V PID parameters * @param error - current error * @param current_time - and current time * @return PID-corrected responce */ static double compute_pid(PIDController *pid, double error, double current_time) { double dt = current_time - pid->prev_time; if(dt <= 0.) dt = 0.01; // Default to 10ms if time isn't tracked // Integral term with anti-windup pid->integral += error * dt; // Clamp integral to ?1000 (adjust based on system limits) if(pid->integral > 1000.) pid->integral = 1000.; if(pid->integral < -1000.) pid->integral = -1000.; // Derivative term (filtered) double derivative = (error - pid->prev_error) / dt; pid->prev_error = error; pid->prev_time = current_time; double pid_out = (pid->Kp * error) + (pid->Ki * pid->integral) + (pid->Kd * derivative); LOGDBG("PID: error=%.2f, integral=%.2f, derivative=%.2f, output=%.2f", error, pid->integral, derivative, pid_out); return pid_out; } /** * @brief try2correct - try to correct position * @param dX - delta of X-coordinate in image space * @param dY - delta of Y-coordinate in image space * @return FALSE if failed (motors are moving etc) or correction out of limits */ static int try2correct(double dX, double dY){ if(!relaxed(Ustepper) || !relaxed(Vstepper)) return FALSE; // calculations: make Ki=0, Kd=0; increase Kp until oscillations; // now Tu - osc period, Ku=Kp for oscillations; so: // Kp = 0.6*Ku; Ki = 1.2*Ku/Tu; Kd = 0.075*Ku*Tu (Ziegler-Nichols) static PIDController pidU = {0}, pidV = {0}; // refresh parameters from configuration pidU.Kp = theconf.PIDU_P; pidU.Ki = theconf.PIDU_I; pidU.Kd = theconf.PIDU_D; pidV.Kp = theconf.PIDV_P; pidV.Ki = theconf.PIDV_I; pidV.Kd = theconf.PIDV_D; double dU, dV; double current_time = sl_dtime(); if( current_time - pidU.prev_time > MAX_PID_TIME || current_time - pidV.prev_time > MAX_PID_TIME){ LOGWARN("Too old PID time: have dt=%gs", current_time - pidU.prev_time); pidU.prev_time = pidV.prev_time = current_time; pidU.integral = pidV.integral = 0.; return FALSE; } // dU = KU*(dX*cosXU + dY*sinXU); dV = KV*(dX*cosXV + dY*sinXV) dU = theconf.Kxu * dX + theconf.Kyu * dY; dV = theconf.Kxv * dX + theconf.Kyv * dY; LOGDBG("dx/dy: %g/%g; dU/dV: %g/%g", dX, dY, dU, dV); // Compute PID outputs double pidU_out = compute_pid(&pidU, dU, current_time); double pidV_out = compute_pid(&pidV, dV, current_time); int usteps = (int)pidU_out, vsteps = (int)pidV_out; int Unew = Uposition + usteps, Vnew = Vposition + vsteps; if(Unew > theconf.maxUpos || Unew < theconf.minUpos || Vnew > theconf.maxVpos || Vnew < theconf.minVpos){ // Reset integral to prevent windup pidU.integral = 0; pidV.integral = 0; // TODO: here we should signal that the limit reached and move by telescope LOGWARN("Correction failed, curpos: %d, %d, should move to %d, %d", Uposition, Vposition, Unew, Vnew); return FALSE; } LOGDBG("try2correct(): move from (%d, %d) to (%d, %d), delta (%.1f, %.1f)", Uposition, Vposition, Unew, Vnew, dU, dV); int ret = TRUE; if(usteps) ret = nth_motor_setter(CMD_RELPOS, Ustepper, usteps); if(vsteps) ret &= nth_motor_setter(CMD_RELPOS, Vstepper, vsteps); if(!ret) LOGWARN("Canserver: cant run corrections"); return ret; } // global variable proc_corr /** * @brief stp_process_corrections - get XY corrections (in pixels) and move motors to fix them * @param X, Y - centroid (x,y) in screen coordinate system * This function called from improc.c each time the corrections calculated (ONLY IF Xtarget/Ytarget > -1) */ static void stp_process_corrections(double X, double Y){ static int coordstrusted = TRUE; if(!relaxed(Ustepper) || !relaxed(Vstepper)){ // don't process coordinates when moving coordstrusted = FALSE; coordsRdy = FALSE; return; } if(!coordstrusted){ // don't trust first coordinates after moving finished coordstrusted = TRUE; coordsRdy = FALSE; return; } //DBG("got centroid data: %g, %g", X, Y); Xtarget = X; Ytarget = Y; coordsRdy = TRUE; } // try to change state; @return TRUE if OK static int stp_setstate(STPstate newstate){ if(newstate == state) return TRUE; if(newstate == STP_DISCONN){ stp_disc(); return TRUE; } if(state == STP_DISCONN){ if(!stp_connect_server()) return FALSE; } if(newstate == STP_SETUP || newstate == STP_GOTOTHEMIDDLE){ sstatus = SETUP_INIT; }else sstatus = SETUP_NONE; state = newstate; return TRUE; } // get current status (global variable stepstatus) // return JSON string with different parameters static char *stp_status(const char *messageid, char *buf, int buflen){ // FNAME(); int l; char *bptr = buf; const char *s = NULL, *stage = NULL; l = snprintf(bptr, buflen, "{ \"%s\": \"%s\", \"status\": ", MESSAGEID, messageid); buflen -= l; bptr += l; switch(state){ case STP_DISCONN: l = snprintf(bptr, buflen, "\"disconnected\""); break; case STP_RELAX: l = snprintf(bptr, buflen, "\"ready\""); break; case STP_SETUP: case STP_GOTOTHEMIDDLE: s = (state == STP_SETUP) ? "setup" : "gotomiddle"; switch(sstatus){ case SETUP_INIT: stage = "init"; break; case SETUP_WAITUV0: stage = "waituv0"; break; case SETUP_WAITUVMID: stage = "waituvmid"; break; case SETUP_WAITU0: stage = "waitu0"; break; case SETUP_WAITUMAX: stage = "waitumax"; break; case SETUP_WAITV0: stage = "waitv0"; break; case SETUP_WAITVMAX: stage = "waitvmax"; break; case SETUP_FINISH: stage = "finishing"; break; default: stage = "unknown"; } l = snprintf(bptr, buflen, "{ \"%s\": \"%s\" }", s, stage); break; case STP_FINDTARGET: l = snprintf(bptr, buflen, "\"findtarget\""); break; case STP_FIX: l = snprintf(bptr, buflen, "\"%s\"", fixerr ? "fixoutofrange" : "fixing"); break; default: l = snprintf(bptr, buflen, "\"unknown\""); } buflen -= l; bptr += l; if(state != STP_DISCONN){ l = snprintf(bptr, buflen, ", "); buflen -= l; bptr += l; for(int i = 0; i < NMOTORS; ++i){ if(!motornames[i]) continue; // this motor not used l = snprintf(bptr, buflen, "\"%s\": { \"status\": \"%s\", \"position\": %d }, ", motornames[i], str_states[motstates[i]], motposition[i]); buflen -= l; bptr += l; } } snprintf(bptr, buflen, " }\n"); return buf; } // commands from client to change status static const char* stringstatuses[STP_STATE_AMOUNT] = { [STP_DISCONN] = "disconnect", [STP_RELAX] = "relax", [STP_SETUP] = "setup", [STP_GOTOTHEMIDDLE] = "middle", [STP_FINDTARGET] = "findtarget", [STP_FIX] = "fix", [STP_UNDEFINED] = "undefined" }; // try to set new status (global variable stepstatus) static char *set_stpstatus(const char *newstatus, char *buf, int buflen){ if(!buf) return NULL; if(!newstatus){ // getter snprintf(buf, buflen, "%s", stringstatuses[state]); return buf; } // FNAME(); STPstate newstate = STP_UNDEFINED; for(int i = 0; i < STP_UNDEFINED; ++i){ if(strcasecmp(stringstatuses[i], newstatus) == 0){ newstate = (STPstate)i; break; } } if(newstate != STP_UNDEFINED){ if(stp_setstate(newstate)){ snprintf(buf, buflen, OK); return buf; }else{ snprintf(buf, buflen, FAIL); return buf; } } int L = snprintf(buf, buflen, "status '%s' undefined, allow: ", newstatus); char *ptr = buf; for(int i = 0; i < STP_UNDEFINED && buflen > 2; ++i){ buflen -= L; ptr += L; L = snprintf(ptr, buflen-2, "'%s' ", stringstatuses[i]); } ptr[L-1] = '\n'; return buf; } // MAIN THREAD static void *stp_process_states(_U_ void *arg){ // FNAME(); static int first = TRUE; // flag for logging when can't reconnect while(!stopwork){ usleep(10000); // check for disconnection flag if(motorsoff){ motorsoff = FALSE; stp_disconnect(); sleep(1); continue; } // check for moving if(state == STP_DISCONN){ DBG("DISCONNECTED - try to connect"); sleep(1); stp_connect_server(); continue; } static double t0 = -1.; if(t0 < 0.) t0 = sl_dtime(); if(state != STP_DISCONN){ if(sl_dtime() - t0 >= 0.1){ // each 0.1s check state if steppers aren't disconnected t0 = sl_dtime(); chkall(); if(!relaxed(Ustepper) && !relaxed(Vstepper)) continue; first = TRUE; }else continue; } // if we are here, all U/V moving is finished // check request to change focus if(chfocus){ DBG("Try to move F to %d", newfocpos); if(nth_motor_setter(CMD_GOTO, Fstepper, newfocpos)){ chfocus = FALSE; } } if(dUmove){ DBG("Try to move U by %d", dUmove); if(nth_motor_setter(CMD_RELPOS, Ustepper, dUmove)){ dUmove = 0; continue; } } if(dVmove){ DBG("Try to move V by %d", dVmove); if(nth_motor_setter(CMD_RELPOS, Vstepper, dVmove)){ dVmove = 0; continue; } } switch(state){ // steppers state machine case STP_DISCONN: if(!stp_connect_server()){ WARNX("Can't reconnect"); if(first){ LOGWARN("Can't reconnect"); first = FALSE; } sleep(1); } break; case STP_SETUP: // setup axes (before this state set Xtarget/Ytarget in improc.c) process_setup_stage(); break; case STP_GOTOTHEMIDDLE: process_movetomiddle_stage(); break; case STP_FINDTARGET: // calculate target coordinates if(coordsRdy){ coordsRdy = FALSE; if(process_targetstage(Xtarget, Ytarget)) state = STP_RELAX; } break; case STP_FIX: // process corrections if(coordsRdy){ coordsRdy = FALSE; DBG("GOT AVERAGE -> correct\n"); double xtg = theconf.xtarget - theconf.xoff, ytg = theconf.ytarget - theconf.yoff; double xdev = xtg - Xtarget, ydev = ytg - Ytarget; double corr = sqrt(xdev*xdev + ydev*ydev); if(theconf.xtarget < 1. || theconf.ytarget < 1. || corr < COORDTOLERANCE){ DBG("Target coordinates not defined or correction too small, targ: (%.1f, %.1f); corr: %.1f, %.1f (abs: %.1f)", theconf.xtarget, theconf.ytarget, xdev, ydev, corr); break; } LOGDBG("Current position: U=%d, V=%d, deviations: dX=%.1f, dy=%.1f", Uposition, Vposition, xdev, ydev); if(!try2correct(xdev, ydev)){ LOGWARN("failed to correct"); fixerr = 1; // TODO: do something here DBG("FAILED"); }else fixerr = 0; } break; default: // STP_RELAX break; } } DBG("thread stopped"); return NULL; } // change focus (global variable movefocus) static char *set_pfocus(const char *newstatus, char *buf, int buflen){ if(!buf) return NULL; if(!newstatus){ // getter snprintf(buf, buflen, "%d", Fposition); return buf; } int newval = atoi(newstatus); if(newval < theconf.minFpos || newval > theconf.maxFpos){ snprintf(buf, buflen, FAIL); LOGDBG("Failed to move F -> %d", newval); DBG("Failed to move F -> %d", newval); }else{ snprintf(buf, buflen, OK); newfocpos = newval; chfocus = TRUE; } return buf; } // move by U and V axis static char *Umove(const char *val, char *buf, int buflen){ if(!buf) return NULL; if(!val){ // getter snprintf(buf, buflen, "%d", Uposition); return buf; } int d = atoi(val); int Unfixed = Uposition + d; if(Unfixed > theconf.maxUpos || Unfixed < theconf.minUpos){ snprintf(buf, buflen, FAIL); LOGDBG("Failed to move U -> %d", Unfixed); DBG("Failed to move U -> %d", Unfixed); return buf; } dUmove = d; snprintf(buf, buflen, OK); return buf; } static char *Vmove(const char *val, char *buf, int buflen){ if(!buf) return NULL; if(!val){ // getter snprintf(buf, buflen, "%d", Vposition); return buf; } int d = atoi(val); int Vnfixed = Vposition + d; if(Vnfixed > theconf.maxVpos || Vnfixed < theconf.minVpos){ snprintf(buf, buflen, FAIL); LOGDBG("Failed to move V -> %d", Vnfixed); DBG("Failed to move V -> %d", Vnfixed); return buf; } dVmove = d; snprintf(buf, buflen, OK); return buf; } static steppersproc steppers = { .stepdisconnect = stp_disc, .proc_corr = stp_process_corrections, .stepstatus = stp_status, .setstepstatus = set_stpstatus, .movefocus = set_pfocus, .moveByU = Umove, .moveByV = Vmove, }; /** * @brief steppers_connect - run a thread processed steppers status * @return FALSE if failed to connect immediately */ steppersproc* steppers_connect(){ DBG("Try to connect"); if(!stp_connect_server()) return NULL; if(pthread_create(&processingthread, NULL, stp_process_states, NULL)){ LOGERR("pthread_create() for steppers server failed"); WARNX("pthread_create()"); return NULL; } return &steppers; }