add PID (not tested yet)

LibSidServo/PID.c

@@ -0,0 +1,174 @@
+/*
+ * This file is part of the libsidservo project.
+ * Copyright 2025 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/>.
+ */
+
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <strings.h>
+
+#include "main.h"
+#include "PID.h"
+#include "serial.h"
+
+PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz){
+    if(!gain || Iarrsz < 3) return NULL;
+    PIDController_t *pid = (PIDController_t*)calloc(1, sizeof(PIDController_t));
+    pid->gain = *gain;
+    pid->pidIarrSize = Iarrsz;
+    pid->pidIarray = (double*)calloc(Iarrsz, sizeof(double));
+    return pid;
+}
+
+// don't clear lastT!
+void pid_clear(PIDController_t *pid){
+    if(!pid) return;
+    DBG("CLEAR PID PARAMETERS");
+    bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize);
+    pid->integral = 0.;
+    pid->prev_error = 0.;
+    pid->curIidx = 0;
+}
+
+void pid_delete(PIDController_t **pid){
+    if(!pid || !*pid) return;
+    if((*pid)->pidIarray) free((*pid)->pidIarray);
+    free(*pid);
+    *pid = NULL;
+}
+
+double pid_calculate(PIDController_t *pid, double error, double dt){
+    // calculate flowing integral
+    double oldi = pid->pidIarray[pid->curIidx], newi = error * dt;
+    DBG("oldi/new: %g, %g", oldi, newi);
+    pid->pidIarray[pid->curIidx++] = newi;
+    if(pid->curIidx >= pid->pidIarrSize) pid->curIidx = 0;
+    pid->integral += newi - oldi;
+    double derivative = (error - pid->prev_error) / dt;
+    pid->prev_error = error;
+    double sum = pid->gain.P * error + pid->gain.I * pid->integral + pid->gain.D * derivative;
+    DBG("P=%g, I=%g, D=%g; sum=%g", pid->gain.P * error, pid->gain.I * pid->integral, pid->gain.D * derivative, sum);
+    return sum;
+}
+
+typedef struct{
+    PIDController_t *PIDC;
+    PIDController_t *PIDV;
+} PIDpair_t;
+
+typedef struct{
+    axis_status_t *state;
+    coordval_t position;
+    coordval_t speed;
+} axisdata_t;
+/**
+ * @brief process - Process PID for given axe
+ * @param tagpos - given coordinate of target position
+ * @param endpoint - endpoint for this coordinate
+ * @param pid - pid itself
+ * @return calculated new speed or -1 for max speed
+ */
+static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t *axis){
+    if(tagpos->t < axis->position.t || tagpos->t - axis->position.t > MCC_PID_MAX_DT) return axis->speed.val; // data is too old or wrong
+    double error = tagpos->val - axis->position.val, fe = fabs(error);
+    PIDController_t *pid = NULL;
+    switch(*axis->state){
+        case AXIS_SLEWING:
+            if(fe < MCC_MAX_POINTING_ERR){
+                *axis->state = AXIS_POINTING;
+                DBG("--> Pointing\n");
+                pid = pidpair->PIDC;
+            }else{
+                DBG("Slewing...\n");
+                return -1.; // max speed for given axis
+            }
+            break;
+        case AXIS_POINTING:
+            if(fe < MCC_MAX_GUIDING_ERR){
+                *axis->state = AXIS_GUIDING;
+                DBG("--> Guiding\n");
+                pid = pidpair->PIDV;
+            }else if(fe > MCC_MAX_POINTING_ERR){
+                DBG("--> Slewing\n");
+                *axis->state = AXIS_SLEWING;
+                return -1.;
+            } else pid = pidpair->PIDC;
+            break;
+        case AXIS_GUIDING:
+            pid = pidpair->PIDV;
+            if(fe > MCC_MAX_GUIDING_ERR){
+                DBG("--> Pointing\n");
+                *axis->state = AXIS_POINTING;
+                pid = pidpair->PIDC;
+            }else if(fe < MCC_MAX_ATTARGET_ERR){
+                DBG("At target\n");
+                // TODO: we can point somehow that we are at target or introduce new axis state
+            }else DBG("Current error: %g\n", fe);
+            break;
+        case AXIS_STOPPED:
+        case AXIS_ERROR:
+            return 0.;
+    }
+    if(!pid){
+        DBG("WTF? Where is a PID?");
+        return axis->speed.val;
+    }
+    if(tagpos->t < pid->prevT || tagpos->t - pid->prevT > MCC_PID_MAX_DT) pid_clear(pid);
+    double dt = tagpos->t - pid->prevT;
+    if(dt > MCC_PID_MAX_DT) dt = MCC_PID_CYCLE_TIME;
+    pid->prevT = tagpos->t;
+    double tagspeed = pid_calculate(pid, error, dt);
+    if(*axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed; // velocity-based
+    return tagspeed; // coordinate-based
+}
+
+/**
+ * @brief correct2 - recalculate PID and move telescope to new point with new speed
+ * @param target - target position (for error calculations)
+ * @param endpoint - stop point (some far enough point to stop in case of hang)
+ * @return error code
+ */
+mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoint){
+    static PIDpair_t pidX = {0}, pidY = {0};
+    if(!pidX.PIDC){
+        pidX.PIDC = pid_create(&Conf.XPIDC, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT);
+        if(!pidX.PIDC) return MCC_E_FATAL;
+        pidX.PIDV = pid_create(&Conf.XPIDV, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT);
+        if(!pidX.PIDV) return MCC_E_FATAL;
+    }
+    if(!pidY.PIDC){
+        pidY.PIDC = pid_create(&Conf.YPIDC, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT);
+        if(!pidY.PIDC) return MCC_E_FATAL;
+        pidY.PIDV = pid_create(&Conf.YPIDV, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT);
+        if(!pidY.PIDV) return MCC_E_FATAL;
+    }
+    mountdata_t m;
+    coordpair_t tagspeed;
+    if(MCC_E_OK != Mount.getMountData(&m)) return MCC_E_FAILED;
+    axisdata_t axe;
+    axe.state = &m.Xstate;
+    axe.position = m.encXposition;
+    axe.speed = m.encXspeed;
+    tagspeed.X = getspeed(&target->X, &pidX, &axe);
+    if(tagspeed.X < 0. || tagspeed.X > MCC_MAX_X_SPEED) tagspeed.X = MCC_MAX_X_SPEED;
+    axe.state = &m.Ystate;
+    axe.position = m.encYposition;
+    axe.speed = m.encYspeed;
+    tagspeed.Y = getspeed(&target->Y, &pidY, &axe);
+    if(tagspeed.Y < 0. || tagspeed.Y > MCC_MAX_Y_SPEED) tagspeed.Y = MCC_MAX_Y_SPEED;
+    return Mount.moveWspeed(endpoint, &tagspeed);
+}

LibSidServo/PID.h

@@ -0,0 +1,40 @@
+/*
+ * This file is part of the libsidservo project.
+ * Copyright 2025 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
+
+#include <stddef.h>
+
+#include "sidservo.h"
+
+typedef struct {
+    PIDpar_t gain;      // PID gains
+    double prev_error;  // Previous error
+    double integral;    // Integral term
+    double *pidIarray;  // array for Integral
+    double prevT;       // time of previous correction
+    size_t pidIarrSize; // it's size
+    size_t curIidx;     // and index of current element
+} PIDController_t;
+
+PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz);
+void pid_clear(PIDController_t *pid);
+void pid_delete(PIDController_t **pid);
+double pid_calculate(PIDController_t *pid, double error, double dt);
+
+mcc_errcodes_t  correct2(const coordval_pair_t *target, const coordpair_t *endpoint);

LibSidServo/TODO

@@ -1,3 +1,2 @@
 1. PID: slew2
-2. add model & config "model ON"
 

LibSidServo/examples/conf.c

@@ -31,6 +31,14 @@ static conf_t Config = {
     .EncoderReqInterval = 0.05,
     .SepEncoder = 2,
     .EncoderSpeedInterval = 0.1,
+    .XPIDC.P = 0.5,
+    .XPIDC.D = 0.05,
+    .XPIDV.P = 0.5,
+    .XPIDV.D = 0.05,
+    .YPIDC.P = 0.5,
+    .YPIDC.D = 0.05,
+    .YPIDV.P = 0.5,
+    .YPIDV.D = 0.05,
 };
 
 static sl_option_t opts[] = {
@@ -45,6 +53,18 @@ static sl_option_t opts[] = {
     {"EncoderYDevPath", NEED_ARG,   NULL,   0,  arg_string, APTR(&Config.EncoderYDevPath),  "path to Y encoder (/dev/encoderY0)"},
     {"EncoderSpeedInterval", NEED_ARG,NULL, 0,  arg_double, APTR(&Config.EncoderSpeedInterval),"interval of speed calculations, s"},
     {"RunModel",        NEED_ARG,   NULL,   0,  arg_int,    APTR(&Config.RunModel),         "instead of real hardware run emulation"},
+    {"XPIDCP",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.XPIDC.P),          "P of X PID (coordinate driven)"},
+    {"XPIDCI",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.XPIDC.I),          "I of X PID (coordinate driven)"},
+    {"XPIDCD",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.XPIDC.D),          "D of X PID (coordinate driven)"},
+    {"YPIDCP",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.YPIDC.P),          "P of Y PID (coordinate driven)"},
+    {"YPIDCI",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.YPIDC.I),          "I of Y PID (coordinate driven)"},
+    {"YPIDCD",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.YPIDC.D),          "D of Y PID (coordinate driven)"},
+    {"XPIDVP",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.XPIDV.P),          "P of X PID (velocity driven)"},
+    {"XPIDVI",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.XPIDV.I),          "I of X PID (velocity driven)"},
+    {"XPIDVD",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.XPIDV.D),          "D of X PID (velocity driven)"},
+    {"YPIDVP",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.YPIDV.P),          "P of Y PID (velocity driven)"},
+    {"YPIDVI",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.YPIDV.I),          "I of Y PID (velocity driven)"},
+    {"YPIDVD",          NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.YPIDV.D),          "D of Y PID (velocity driven)"},
     end_option
 };
 

LibSidServo/libsidservo.files

@@ -1,4 +1,6 @@
 CMakeLists.txt
+PID.c
+PID.h
 examples/SSIIconf.c
 examples/conf.c
 examples/conf.h

LibSidServo/main.c

@@ -30,6 +30,7 @@
 #include "movingmodel.h"
 #include "serial.h"
 #include "ssii.h"
+#include "PID.h"
 
 conf_t Conf = {0};
 // parameters for model
@@ -209,17 +210,18 @@ static int chkYs(double s){
     return TRUE;
 }
 
+/*
 static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
     (void)target;
     (void)flags;
     //if(Conf.RunModel) return ... ;
     if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
     //...
-    setStat(MNT_SLEWING, MNT_SLEWING);
+    setStat(AXIS_SLEWING, AXIS_SLEWING);
     //...
     return MCC_E_FAILED;
 }
-
+*/
 
 /**
  * @brief move2 - simple move to given point and stop
@@ -239,7 +241,7 @@ static mcc_errcodes_t move2(const coordpair_t *target){
     cmd.Yspeed = MCC_MAX_Y_SPEED;
     mcc_errcodes_t r = shortcmd(&cmd);
     if(r != MCC_E_OK) return r;
-    setStat(MNT_SLEWING, MNT_SLEWING);
+    setStat(AXIS_SLEWING, AXIS_SLEWING);
     return MCC_E_OK;
 }
 
@@ -269,14 +271,6 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
     if(!target || !speed) return MCC_E_BADFORMAT;
     if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT;
     if(!chkXs(speed->X) || !chkYs(speed->Y)) return MCC_E_BADFORMAT;
-   /* if(Conf.RunModel){
-        double curt = nanotime();
-        moveparam_t param = {0};
-        param.coord = target->X; param.speed = speed->X;
-        if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED;
-        param.coord = target->Y; param.speed = speed->Y;
-        if(!model_move2(Ymodel, &param, curt)) return MCC_E_FAILED;
-    }*/
     if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
     short_command_t cmd = {0};
     cmd.Xmot = target->X;
@@ -285,7 +279,7 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
     cmd.Yspeed = speed->Y;
     mcc_errcodes_t r = shortcmd(&cmd);
     if(r != MCC_E_OK) return r;
-    setStat(MNT_SLEWING, MNT_SLEWING);
+    setStat(AXIS_SLEWING, AXIS_SLEWING);
     return MCC_E_OK;
 }
 
@@ -501,7 +495,7 @@ mount_t Mount = {
     .init = init,
     .quit = quit,
     .getMountData = getMD,
-    .slewTo = slew2,
+//    .slewTo = slew2,
     .moveTo = move2,
     .moveWspeed = move2s,
     .setSpeed = setspeed,
@@ -512,5 +506,6 @@ mount_t Mount = {
     .getHWconfig = get_hwconf,
     .saveHWconfig = write_hwconf,
     .currentT = nanotime,
+    .correctTo = correct2,
 };
 

LibSidServo/serial.c

@@ -585,10 +585,10 @@ mcc_errcodes_t getMD(mountdata_t  *d){
     return MCC_E_OK;
 }
 
-void setStat(mnt_status_t Xstatus, mnt_status_t Ystatus){
+void setStat(axis_status_t Xstate, axis_status_t Ystate){
     pthread_mutex_lock(&datamutex);
-    mountdata.Xstatus = Xstatus;
+    mountdata.Xstate = Xstate;
-    mountdata.Ystatus = Ystatus;
+    mountdata.Ystate = Ystate;
     pthread_mutex_unlock(&datamutex);
 }
 

LibSidServo/serial.h

@@ -34,7 +34,7 @@ int openEncoder();
 int openMount();
 void closeSerial();
 mcc_errcodes_t getMD(mountdata_t  *d);
-void setStat(mnt_status_t Xstatus, mnt_status_t Ystatus);
+void setStat(axis_status_t Xstate, axis_status_t Ystate);
 int MountWriteRead(const data_t *out, data_t *in);
 int MountWriteReadRaw(const data_t *out, data_t *in);
 int cmdS(SSscmd *cmd);

LibSidServo/sidservo.h

@@ -49,6 +49,19 @@ extern "C"
 #define MCC_CONF_MAX_SPEEDINT   (2.)
 // minimal speed interval in parts of EncoderReqInterval
 #define MCC_CONF_MIN_SPEEDC     (3.)
+// PID I cycle time (analog of "RC" for PID on opamps)
+#define MCC_PID_CYCLE_TIME      (5.)
+// maximal PID refresh time interval (if larger all old data will be cleared)
+#define MCC_PID_MAX_DT          (1.)
+// normal PID refresh interval
+#define MCC_PID_REFRESH_DT      (0.1)
+// boundary conditions for axis state: "slewing/pointing/guiding"
+// if angle < MCC_MAX_POINTING_ERR, change state from "slewing" to "pointing": 12 degrees
+#define MCC_MAX_POINTING_ERR    (0.20943951)
+// if angle < MCC_MAX_GUIDING_ERR, chane state from "pointing" to "slewing": 15''
+#define MCC_MAX_GUIDING_ERR     (7.272205e-5)
+// if error less than this value we suppose that target is captured and guiding is good: 0.1''
+#define MCC_MAX_ATTARGET_ERR    (4.8481368e-7)
 
 // error codes
 typedef enum{
@@ -61,17 +74,25 @@ typedef enum{
 } mcc_errcodes_t;
 
 typedef struct{
-    char*   MountDevPath;       // path to mount device
+    double P, I, D;
-    int     MountDevSpeed;      // serial speed
+} PIDpar_t;
-    char*   EncoderDevPath;     // path to encoder device
+
-    int     EncoderDevSpeed;    // serial speed
+typedef struct{
-    int     SepEncoder;         // ==1 if encoder works as separate serial device, ==2 if there's new version with two devices
+    char*   MountDevPath;           // path to mount device
-    char*   EncoderXDevPath;    // paths to new controller devices
+    int     MountDevSpeed;          // serial speed
+    char*   EncoderDevPath;         // path to encoder device
+    int     EncoderDevSpeed;        // serial speed
+    int     SepEncoder;             // ==1 if encoder works as separate serial device, ==2 if there's new version with two devices
+    char*   EncoderXDevPath;        // paths to new controller devices
     char*   EncoderYDevPath;
-    double  MountReqInterval;   // interval between subsequent mount requests (seconds)
+    double  MountReqInterval;       // interval between subsequent mount requests (seconds)
-    double  EncoderReqInterval; // interval between subsequent encoder requests (seconds)
+    double  EncoderReqInterval;     // interval between subsequent encoder requests (seconds)
-    double  EncoderSpeedInterval;// interval between speed calculations
+    double  EncoderSpeedInterval;   // interval between speed calculations
-    int     RunModel;            // == 1 if you want to use model instead of real mount
+    int     RunModel;               // == 1 if you want to use model instead of real mount
+    PIDpar_t XPIDC;                 // gain parameters of PID for both axes (C - coordinate driven, V - velocity driven)
+    PIDpar_t XPIDV;
+    PIDpar_t YPIDC;
+    PIDpar_t YPIDV;
 } conf_t;
 
 // coordinates/speeds in degrees or d/s: X, Y
@@ -128,16 +149,16 @@ typedef struct{
 } extradata_t;
 
 typedef enum{
-    MNT_STOPPED,
+    AXIS_STOPPED,
-    MNT_SLEWING,
+    AXIS_SLEWING,
-    MNT_POINTING,
+    AXIS_POINTING,
-    MNT_GUIDING,
+    AXIS_GUIDING,
-    MNT_ERROR,
+    AXIS_ERROR,
-} mnt_status_t;
+} axis_status_t;
 
 typedef struct{
-    mnt_status_t Xstatus;
+    axis_status_t Xstate;
-    mnt_status_t Ystatus;
+    axis_status_t Ystate;
     coordval_t motXposition;
     coordval_t motYposition;
     coordval_t encXposition;
@@ -212,19 +233,19 @@ typedef struct{
     double backlspd;    // Backlash speed (rad/s)
 } hardware_configuration_t;
 
-// flags for slew function
+/* flags for slew function
 typedef struct{
     uint32_t slewNguide : 1; // ==1 to guide after slewing
 } slewflags_t;
-
+*/
 // mount class
 typedef struct{
     // TODO: on init/quit clear all XY-bits to default`
     mcc_errcodes_t  (*init)(conf_t *c); // init device
     void            (*quit)(); // deinit
     mcc_errcodes_t  (*getMountData)(mountdata_t *d); // get last data
-    mcc_errcodes_t  (*slewTo)(const coordpair_t *target, slewflags_t flags);
+//    mcc_errcodes_t  (*slewTo)(const coordpair_t *target, slewflags_t flags);
-    mcc_errcodes_t  (*correctTo)(coordval_pair_t *target);
+    mcc_errcodes_t  (*correctTo)(const coordval_pair_t *target, const coordpair_t *endpoint);
     mcc_errcodes_t  (*moveTo)(const coordpair_t *target); // move to given position and stop
     mcc_errcodes_t  (*moveWspeed)(const coordpair_t *target, const  coordpair_t *speed); // move with given max speed
     mcc_errcodes_t  (*setSpeed)(const coordpair_t *tagspeed); // set speed

LibSidServo/ssii.c

@@ -37,18 +37,18 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
 }
 
 
-static void axestat(int32_t *prev, int32_t cur, int *nstopped, mnt_status_t *stat){
+static void axestat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t *stat){
     if(*prev == INT32_MAX){
-        *stat = MNT_STOPPED;
+        *stat = AXIS_STOPPED;
         DBG("START");
-    }else if(*stat != MNT_STOPPED){
+    }else if(*stat != AXIS_STOPPED){
         if(*prev == cur  && ++(*nstopped) > MOTOR_STOPPED_CNT){
-            *stat = MNT_STOPPED;
+            *stat = AXIS_STOPPED;
             DBG("AXE stopped");
         }
     }else if(*prev != cur){
         DBG("AXE moving");
-        //*stat = MNT_SLEWING;
+        //*stat = AXIS_SLEWING;
         *nstopped = 0;
     }
     *prev = cur;
@@ -57,8 +57,8 @@ static void axestat(int32_t *prev, int32_t cur, int *nstopped, mnt_status_t *sta
 static void ChkStopped(const SSstat *s, mountdata_t *m){
     static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates
     static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state
-    axestat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstatus);
+    axestat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstate);
-    axestat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystatus);
+    axestat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystate);
 }
 
 /**
@@ -190,14 +190,14 @@ mcc_errcodes_t updateMotorPos(){
             }
             DBG("got; t pos x/y: %g/%g; tnow: %g", md.encXposition.t, md.encYposition.t, t);
             mcc_errcodes_t OK = MCC_E_OK;
-            if(fabs(md.motXposition.val - md.encXposition.val) > MCC_ENCODERS_ERROR && md.Xstatus == MNT_STOPPED){
+            if(fabs(md.motXposition.val - md.encXposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){
                 DBG("NEED to sync X: motors=%g, axes=%g", md.motXposition.val, md.encXposition.val);
                 if(!SSsetterI(CMD_MOTXSET, X_RAD2MOT(md.encXposition.val))){
                     DBG("Xpos sync failed!");
                     OK = MCC_E_FAILED;
                 }else DBG("Xpos sync OK, Dt=%g", nanotime() - t0);
             }
-            if(fabs(md.motYposition.val - md.encYposition.val) > MCC_ENCODERS_ERROR && md.Xstatus == MNT_STOPPED){
+            if(fabs(md.motYposition.val - md.encYposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){
                 DBG("NEED to sync Y: motors=%g, axes=%g", md.motYposition.val, md.encYposition.val);
                 if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){
                     DBG("Ypos sync failed!");