diff --git a/LibSidServo/PID.c b/LibSidServo/PID.c new file mode 100644 index 0000000..8c8cd7d --- /dev/null +++ b/LibSidServo/PID.c @@ -0,0 +1,174 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 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 . + */ + +#include +#include +#include +#include + +#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); +} diff --git a/LibSidServo/PID.h b/LibSidServo/PID.h new file mode 100644 index 0000000..5eb8cb9 --- /dev/null +++ b/LibSidServo/PID.h @@ -0,0 +1,40 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 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 . + */ + +#pragma once + +#include + +#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); diff --git a/LibSidServo/TODO b/LibSidServo/TODO index 98925fa..9dae84f 100644 --- a/LibSidServo/TODO +++ b/LibSidServo/TODO @@ -1,3 +1,2 @@ 1. PID: slew2 -2. add model & config "model ON" diff --git a/LibSidServo/examples/conf.c b/LibSidServo/examples/conf.c index 31662d1..89abb73 100644 --- a/LibSidServo/examples/conf.c +++ b/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 }; diff --git a/LibSidServo/libsidservo.files b/LibSidServo/libsidservo.files index 7b0b53e..7777ac7 100644 --- a/LibSidServo/libsidservo.files +++ b/LibSidServo/libsidservo.files @@ -1,4 +1,6 @@ CMakeLists.txt +PID.c +PID.h examples/SSIIconf.c examples/conf.c examples/conf.h diff --git a/LibSidServo/main.c b/LibSidServo/main.c index 534e036..b0050ea 100644 --- a/LibSidServo/main.c +++ b/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, ¶m, curt)) return MCC_E_FAILED; - param.coord = target->Y; param.speed = speed->Y; - if(!model_move2(Ymodel, ¶m, 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, }; diff --git a/LibSidServo/serial.c b/LibSidServo/serial.c index 74eaf61..400213b 100644 --- a/LibSidServo/serial.c +++ b/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.Ystatus = Ystatus; + mountdata.Xstate = Xstate; + mountdata.Ystate = Ystate; pthread_mutex_unlock(&datamutex); } diff --git a/LibSidServo/serial.h b/LibSidServo/serial.h index dd4a95d..45694b1 100644 --- a/LibSidServo/serial.h +++ b/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); diff --git a/LibSidServo/sidservo.h b/LibSidServo/sidservo.h index f8ab95c..9e9f468 100644 --- a/LibSidServo/sidservo.h +++ b/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 - 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 + double P, I, D; +} PIDpar_t; + +typedef struct{ + char* MountDevPath; // path to mount device + 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 EncoderReqInterval; // interval between subsequent encoder requests (seconds) - double EncoderSpeedInterval;// interval between speed calculations - int RunModel; // == 1 if you want to use model instead of real mount + double MountReqInterval; // interval between subsequent mount requests (seconds) + double EncoderReqInterval; // interval between subsequent encoder requests (seconds) + double EncoderSpeedInterval; // interval between speed calculations + 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, - MNT_SLEWING, - MNT_POINTING, - MNT_GUIDING, - MNT_ERROR, -} mnt_status_t; + AXIS_STOPPED, + AXIS_SLEWING, + AXIS_POINTING, + AXIS_GUIDING, + AXIS_ERROR, +} axis_status_t; typedef struct{ - mnt_status_t Xstatus; - mnt_status_t Ystatus; + axis_status_t Xstate; + 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 (*correctTo)(coordval_pair_t *target); +// mcc_errcodes_t (*slewTo)(const coordpair_t *target, slewflags_t flags); + 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 diff --git a/LibSidServo/ssii.c b/LibSidServo/ssii.c index b76faff..51c6faa 100644 --- a/LibSidServo/ssii.c +++ b/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(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystatus); + axestat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstate); + 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!");