introduce a lot of errors when trying to apply model

LibSidServo/main.c

@@ -16,28 +16,82 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+/*
+ * main functions to fill struct `mount_t`
+ */
+
 #include <inttypes.h>
+#include <strings.h>
+#include <time.h>
 #include <stdio.h>
 #include <stdlib.h>
 
-#include "dbg.h"
+#include "main.h"
+#include "movingmodel.h"
 #include "serial.h"
 #include "ssii.h"
 
 conf_t Conf = {0};
-
+// parameters for model
+static movemodel_t *Xmodel, *Ymodel;
+// radians, rad/sec, rad/sec^2
+static limits_t
+    Xlimits = {
+        .min = {.coord = -3.1241, .speed = 1e-8, .accel = 1e-6},
+        .max = {.coord = 3.1241, .speed = MCC_MAX_X_SPEED, .accel = MCC_X_ACCELERATION}},
+    Ylimits = {
+        .min = {.coord = -3.1241, .speed = 1e-8, .accel = 1e-6},
+        .max = {.coord = 3.1241, .speed = MCC_MAX_Y_SPEED, .accel = MCC_Y_ACCELERATION}}
+;
 static mcc_errcodes_t shortcmd(short_command_t *cmd);
 
+/**
+ * @brief nanotime - monotonic time from first run
+ * @return time in seconds
+ */
+double nanotime(){
+    static struct timespec *start = NULL;
+    struct timespec now;
+    if(!start){
+        start = malloc(sizeof(struct timespec));
+        if(!start) return -1.;
+        if(clock_gettime(CLOCK_MONOTONIC, start)) return -1.;
+    }
+    if(clock_gettime(CLOCK_MONOTONIC, &now)) return -1.;
+    double nd = ((double)now.tv_nsec - (double)start->tv_nsec) * 1e-9;
+    double sd = (double)now.tv_sec - (double)start->tv_sec;
+    return sd + nd;
+}
+
 /**
  * @brief quit - close all opened and return to default state
  */
 static void quit(){
-    DBG("Close serial devices");
+    if(Conf.RunModel) return;
     for(int i = 0; i < 10; ++i) if(SSstop(TRUE)) break;
+    DBG("Close all serial devices");
     closeSerial();
     DBG("Exit");
 }
 
+void getModData(mountdata_t *mountdata){
+    if(!mountdata || !Xmodel || !Ymodel) return;
+    double tnow = nanotime();
+    moveparam_t Xp, Yp;
+    movestate_t Xst = Xmodel->get_state(&Xp);
+    if(Xst == ST_MOVE) Xst = Xmodel->proc_move(&Xp, tnow);
+    movestate_t Yst = Ymodel->get_state(&Yp);
+    if(Yst == ST_MOVE) Yst = Ymodel->proc_move(&Yp, tnow);
+    bzero(mountdata, sizeof(mountdata_t));
+    mountdata->motXposition.t = mountdata->encXposition.t = mountdata->motYposition.t = mountdata->encYposition.t = tnow;
+    mountdata->motXposition.val = mountdata->encXposition.val  = Xp.coord;
+    mountdata->motYposition.val  = mountdata->encYposition.val  = Yp.coord;
+    mountdata->encXspeed.t = mountdata->encYspeed.t = tnow;
+    mountdata->encXspeed.val = Xp.speed;
+    mountdata->encYspeed.val = Yp.speed;
+    mountdata->millis = (uint32_t)(tnow * 1e3);
+}
+
 /**
  * @brief init - open serial devices and do other job
  * @param c - initial configuration
@@ -48,6 +102,12 @@ static mcc_errcodes_t init(conf_t *c){
     if(!c) return MCC_E_BADFORMAT;
     Conf = *c;
     mcc_errcodes_t ret = MCC_E_OK;
+    Xmodel = model_init(&Xlimits);
+    Ymodel = model_init(&Ylimits);
+    if(Conf.RunModel){
+        if(!Xmodel || !Ymodel) return MCC_E_FAILED;
+        return MCC_E_OK;
+    }
     if(!Conf.MountDevPath || Conf.MountDevSpeed < 1200){
         DBG("Define mount device path and speed");
         ret = MCC_E_BADFORMAT;
@@ -102,6 +162,7 @@ static int chkYs(double s){
 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);
@@ -119,6 +180,14 @@ static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
 static mcc_errcodes_t move2(const coordpair_t *target){
     if(!target) return MCC_E_BADFORMAT;
     if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT;
+    if(Conf.RunModel){
+        double curt = nanotime();
+        moveparam_t param = {0};
+        param.coord = target->X; param.speed = MCC_MAX_X_SPEED;
+        if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED;
+        param.coord = target->Y; param.speed = MCC_MAX_Y_SPEED;
+        if(!model_move2(Ymodel, &param, curt)) return MCC_E_FAILED;
+    }
     if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
     short_command_t cmd = {0};
     DBG("x,y: %g, %g", target->X, target->Y);
@@ -140,6 +209,7 @@ static mcc_errcodes_t move2(const coordpair_t *target){
  */
 static mcc_errcodes_t setspeed(const coordpair_t *tagspeed){
     if(!tagspeed || !chkXs(tagspeed->X) || !chkYs(tagspeed->Y)) return MCC_E_BADFORMAT;
+    if(Conf.RunModel) return MCC_E_FAILED;
     int32_t spd = X_RS2MOTSPD(tagspeed->X);
     if(!SSsetterI(CMD_SPEEDX, spd)) return MCC_E_FAILED;
     spd = Y_RS2MOTSPD(tagspeed->Y);
@@ -157,6 +227,14 @@ 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;
@@ -174,11 +252,23 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
  * @return errcode
  */
 static mcc_errcodes_t emstop(){
+    if(Conf.RunModel){
+        double curt = nanotime();
+        Xmodel->emergency_stop(curt);
+        Ymodel->emergency_stop(curt);
+        return MCC_E_OK;
+    }
     if(!SSstop(TRUE)) return MCC_E_FAILED;
     return MCC_E_OK;
 }
 // normal stop
 static mcc_errcodes_t stop(){
+    if(Conf.RunModel){
+        double curt = nanotime();
+        Xmodel->stop(curt);
+        Ymodel->stop(curt);
+        return MCC_E_OK;
+    }
     if(!SSstop(FALSE)) return MCC_E_FAILED;
     return MCC_E_OK;
 }
@@ -190,6 +280,7 @@ static mcc_errcodes_t stop(){
  */
 static mcc_errcodes_t shortcmd(short_command_t *cmd){
     if(!cmd) return MCC_E_BADFORMAT;
+    if(Conf.RunModel) return MCC_E_FAILED;
     SSscmd s = {0};
     DBG("tag: xmot=%g rad, ymot=%g rad", cmd->Xmot, cmd->Ymot);
     s.Xmot = X_RAD2MOT(cmd->Xmot);
@@ -205,12 +296,13 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd){
 }
 
 /**
- * @brief shortcmd - send and receive long binary command
+ * @brief longcmd - send and receive long binary command
  * @param cmd (io) - command
  * @return errcode
  */
 static mcc_errcodes_t longcmd(long_command_t *cmd){
     if(!cmd) return MCC_E_BADFORMAT;
+    if(Conf.RunModel) return MCC_E_FAILED;
     SSlcmd l = {0};
     l.Xmot = X_RAD2MOT(cmd->Xmot);
     l.Ymot = Y_RAD2MOT(cmd->Ymot);
@@ -226,6 +318,7 @@ static mcc_errcodes_t longcmd(long_command_t *cmd){
 
 static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
     if(!hwConfig) return MCC_E_BADFORMAT;
+    if(Conf.RunModel) return MCC_E_FAILED;
     SSconfig config;
     if(!cmdC(&config, FALSE)) return MCC_E_FAILED;
     // Convert acceleration (ticks per loop^2 to rad/s^2)
@@ -266,8 +359,8 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
     // Copy ticks per revolution
     hwConfig->Xsetpr = __bswap_32(config.Xsetpr);
     hwConfig->Ysetpr = __bswap_32(config.Ysetpr);
-    hwConfig->Xmetpr = __bswap_32(config.Xmetpr);
+    hwConfig->Xmetpr = __bswap_32(config.Xmetpr) / 4; // as documentation said, real ticks are 4 times less
-    hwConfig->Ymetpr = __bswap_32(config.Ymetpr);
+    hwConfig->Ymetpr = __bswap_32(config.Ymetpr) / 4;
     // Convert slew rates (ticks per loop to rad/s)
     hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate);
     hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate);
@@ -290,6 +383,7 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
 
 static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){
     SSconfig config;
+    if(Conf.RunModel) return MCC_E_FAILED;
     // Convert acceleration (rad/s^2 to ticks per loop^2)
     config.Xconf.accel = X_RS2MOTACC(hwConfig->Xconf.accel);
     config.Yconf.accel = Y_RS2MOTACC(hwConfig->Yconf.accel);
@@ -359,5 +453,5 @@ mount_t Mount = {
     .longCmd = longcmd,
     .getHWconfig = get_hwconf,
     .saveHWconfig = write_hwconf,
-    .currentT = dtime,
+    .currentT = nanotime,
 };

LibSidServo/main.h

@@ -0,0 +1,70 @@
+/*
+ * 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/>.
+ */
+
+/*
+ * Almost all here used for debug purposes
+ */
+
+#pragma once
+
+#include <stdlib.h>
+
+#include "sidservo.h"
+
+extern conf_t Conf;
+double nanotime();
+void getModData(mountdata_t *mountdata);
+
+// unused arguments of functions
+#define _U_         __attribute__((__unused__))
+// break absent in `case`
+#define FALLTHRU    __attribute__ ((fallthrough))
+// and synonym for FALLTHRU
+#define NOBREAKHERE __attribute__ ((fallthrough))
+// weak functions
+#define WEAK        __attribute__ ((weak))
+
+#ifndef DBL_EPSILON
+#define DBL_EPSILON        (2.2204460492503131e-16)
+#endif
+
+#ifndef FALSE
+#define FALSE (0)
+#endif
+
+#ifndef TRUE
+#define TRUE (1)
+#endif
+
+
+#ifdef EBUG
+#include <stdio.h>
+    #define COLOR_RED           "\033[1;31;40m"
+    #define COLOR_GREEN         "\033[1;32;40m"
+    #define COLOR_OLD           "\033[0;0;0m"
+    #define FNAME() do{ fprintf(stderr, COLOR_GREEN "\n%s " COLOR_OLD, __func__); \
+    fprintf(stderr, "(%s, line %d)\n", __FILE__, __LINE__);} while(0)
+    #define DBG(...) do{ fprintf(stderr, COLOR_RED "\n%s " COLOR_OLD, __func__); \
+        fprintf(stderr, "(%s, line %d): ", __FILE__, __LINE__); \
+        fprintf(stderr, __VA_ARGS__);           \
+        fprintf(stderr, "\n");} while(0)
+
+#else  // EBUG
+    #define FNAME()
+    #define DBG(...)
+#endif // EBUG

LibSidServo/movingmodel.c

@@ -0,0 +1,63 @@
+/*
+ * This file is part of the moving_model 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 <string.h>
+#include <pthread.h>
+
+#include "main.h"
+#include "movingmodel.h"
+#include "ramp.h"
+
+extern movemodel_t trapez;
+
+static void chkminmax(double *min, double *max){
+    if(*min <= *max) return;
+    double t = *min;
+    *min = *max;
+    *max = t;
+}
+
+movemodel_t *model_init(limits_t *l){
+    if(!l) return FALSE;
+    movemodel_t *model = calloc(1, sizeof(movemodel_t));
+    memcpy(model, &trapez, sizeof(movemodel_t));
+    if(!model->init_limits) goto fail;
+    moveparam_t *max = &l->max, *min = &l->min;
+    if(min->speed < 0.) min->speed = -min->speed;
+    if(max->speed < 0.) max->speed = -max->speed;
+    if(min->accel < 0.) min->accel = -min->accel;
+    if(max->accel < 0.) max->accel = -max->accel;
+    chkminmax(&min->coord, &max->coord);
+    chkminmax(&min->speed, &max->speed);
+    chkminmax(&min->accel, &max->accel);
+    if(!model->init_limits(l)) return NULL;
+    return model;
+fail:
+    free(model); return NULL;
+}
+
+int model_move2(movemodel_t *model, moveparam_t *target, double t){
+    if(!target || !model) return FALSE;
+    DBG("MOVE to %g at speed %g", target->coord, target->speed);
+    // only positive velocity
+    if(target->speed < 0.) target->speed = -target->speed;
+    // don't mind about acceleration - user cannot set it now
+    return model->calculate(target, t);
+}

LibSidServo/movingmodel.h

@@ -0,0 +1,60 @@
+/*
+ * This file is part of the moving_model 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 "sidservo.h"
+
+// tolerance, time ticks
+#define COORD_TOLERANCE_DEFAULT (0.01)
+#define COORD_TOLERANCE_MIN     (0.0001)
+#define COORD_TOLERANCE_MAX     (10.)
+#define TIME_TICK_DEFAULT       (0.0001)
+#define TIME_TICK_MIN           (1e-9)
+#define TIME_TICK_MAX           (10.)
+
+typedef enum{
+    ST_STOP,            // stopped
+    ST_MOVE,            // moving
+    ST_AMOUNT
+} movestate_t;
+
+typedef struct{
+    double coord;
+    double speed;
+    double accel;
+} moveparam_t;
+
+typedef struct{
+    moveparam_t min;
+    moveparam_t max;
+    double acceleration;
+} limits_t;
+
+typedef struct{
+    int (*init_limits)(limits_t *lim);                      // init values of limits, jerk
+    int (*calculate)(moveparam_t *target, double t);        // calculate stages of traectory beginning from t
+    movestate_t (*proc_move)(moveparam_t *next, double t);  // calculate next model point for time t
+    movestate_t (*get_state)(moveparam_t *cur);             // get current moving state
+    void (*stop)(double t);                                 // stop by ramp
+    void (*emergency_stop)(double t);                       // stop with highest acceleration
+    double (*stoppenanotime)();                                // time when moving will ends
+} movemodel_t;
+
+movemodel_t *model_init(limits_t *l);
+int model_move2(movemodel_t *model, moveparam_t *target, double t);

LibSidServo/ramp.c

@@ -0,0 +1,220 @@
+/*
+ * This file is part of the moving_model 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/>.
+ */
+
+// simplest trapezioidal ramp
+
+#include <math.h>
+#include <strings.h>
+
+#include "main.h"
+#include "ramp.h"
+
+static double coord_tolerance = COORD_TOLERANCE_DEFAULT;
+static movestate_t state = ST_STOP;
+static moveparam_t Min, Max;
+
+typedef enum{
+    STAGE_ACCEL,        // start from zero speed and accelerate to Max speed
+    STAGE_MAXSPEED,     // go with target speed
+    STAGE_DECEL,        // go from target speed to zero
+    STAGE_STOPPED,      // stop
+    STAGE_AMOUNT
+} movingstage_t;
+
+static movingstage_t movingstage = STAGE_STOPPED;
+static double Times[STAGE_AMOUNT] = {0}; // time when each stage starts
+static moveparam_t Params[STAGE_AMOUNT] = {0}; // starting parameters for each stage
+static moveparam_t curparams = {0}; // current coordinate/speed/acceleration
+
+static int initlims(limits_t *lim){
+    if(!lim) return FALSE;
+    Min = lim->min;
+    Max = lim->max;
+    return TRUE;
+}
+
+static void emstop(double _U_ t){
+    curparams.accel = 0.;
+    curparams.speed = 0.;
+    bzero(Times, sizeof(Times));
+    bzero(Params, sizeof(Params));
+    state = ST_STOP;
+    movingstage = STAGE_STOPPED;
+}
+
+static void stop(double t){
+    if(state == ST_STOP || movingstage == STAGE_STOPPED) return;
+    movingstage = STAGE_DECEL;
+    state = ST_MOVE;
+    Times[STAGE_DECEL] = t;
+    Params[STAGE_DECEL].speed = curparams.speed;
+    if(curparams.speed > 0.) Params[STAGE_DECEL].accel = -Max.accel;
+    else Params[STAGE_DECEL].accel = Max.accel;
+    Params[STAGE_DECEL].coord = curparams.coord;
+    // speed: v=v2+a2(t-t2), v2 and a2 have different signs; t3: v3=0 -> t3=t2-v2/a2
+    Times[STAGE_STOPPED] = t - curparams.speed / Params[STAGE_DECEL].accel;
+    // coordinate: x=x2+v2(t-t2)+a2(t-t2)^2/2 -> x3=x2+v2(t3-t2)+a2(t3-t2)^2/2
+    double dt = Times[STAGE_STOPPED] - t;
+    Params[STAGE_STOPPED].coord = curparams.coord + curparams.speed * dt +
+                                  Params[STAGE_DECEL].accel * dt * dt / 2.;
+}
+
+/**
+ * @brief calc - moving calculation
+ * @param x - using max speed (>0!!!) and coordinate
+ * @param t - current time value
+ * @return FALSE if can't move with given parameters
+ */
+static int calc(moveparam_t *x, double t){
+    if(!x) return FALSE;
+    if(x->coord < Min.coord || x->coord > Max.coord) return FALSE;
+    if(x->speed < Min.speed || x->speed > Max.speed) return FALSE;
+    double Dx = fabs(x->coord - curparams.coord); // full distance
+    double sign = (x->coord > curparams.coord) ? 1. : -1.; // sign of target accelerations and speeds
+    // we have two variants: with or without stage with constant speed
+    double dt23 = x->speed / Max.accel; // time of deceleration stage for given speed
+    double dx23 = x->speed * dt23 / 2.; // distance on dec stage (abs)
+    DBG("Dx=%g, sign=%g, dt23=%g, dx23=%g", Dx, sign, dt23, dx23);
+    double setspeed = x->speed; // new max speed (we can change it if need)
+    double dt01, dx01; // we'll fill them depending on starting conditions
+    Times[0] = t;
+    Params[0].speed = curparams.speed;
+    Params[0].coord = curparams.coord;
+
+    double curspeed = fabs(curparams.speed);
+    double dt0s = curspeed / Max.accel; // time of stopping phase
+    double dx0s = curspeed * dt0s / 2.; // distance
+    DBG("dt0s=%g, dx0s=%g", dt0s, dx0s);
+    if(dx0s > Dx){
+        DBG("distance too short");
+        return FALSE;
+    }
+    if(fabs(Dx - dx0s) < coord_tolerance){ // just stop and we'll be on target
+        DBG("Distance good to just stop");
+        stop(t);
+        return TRUE;
+    }
+    if(curparams.speed * sign < 0. || state == ST_STOP){ // we should change speed sign
+        // after stop we will have full profile
+        double dxs3 = Dx - dx0s;
+        double newspeed = sqrt(Max.accel * dxs3);
+        if(newspeed < setspeed) setspeed = newspeed; // we can't reach user speed
+        DBG("dxs3=%g, setspeed=%g", dxs3, setspeed);
+        dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel;
+        Params[0].accel = sign * Max.accel;
+        if(state == ST_STOP) dx01 = setspeed * dt01 / 2.;
+        else dx01 = dt01 * (dt01 / 2. * Max.accel - curspeed);
+        DBG("dx01=%g, dt01=%g", dx01, dt01);
+    }else{ // increase or decrease speed without stopping phase
+        dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel;
+        double a = sign * Max.accel;
+        if(sign * curparams.speed < 0.){DBG("change direction"); a = -a;}
+        else if(curspeed > setspeed){ DBG("lower speed @ this direction"); a = -a;}
+        //double a = (curspeed > setspeed) ? -Max.accel : Max.accel;
+        dx01 = curspeed * dt01 + a * dt01 * dt01 / 2.;
+        DBG("dt01=%g, a=%g, dx01=%g", dt01, a, dx01);
+        if(dx01 + dx23 > Dx){ // calculate max speed
+            setspeed = sqrt(Max.accel * Dx - curspeed * curspeed / 2.);
+            if(setspeed < curspeed){
+                setspeed = curparams.speed;
+                dt01 = 0.; dx01 = 0.;
+                Params[0].accel = 0.;
+            }else{
+                Params[0].accel = a;
+                dt01 = fabs(setspeed - curspeed) / Max.accel;
+                dx01 = curspeed * dt01 + Max.accel * dt01 * dt01 / 2.;
+            }
+        }else Params[0].accel = a;
+    }
+    if(setspeed < Min.speed){
+        DBG("New speed should be too small");
+        return FALSE;
+    }
+    moveparam_t *p = &Params[STAGE_MAXSPEED];
+    p->accel = 0.; p->speed = sign * setspeed;
+    p->coord = curparams.coord + dx01 * sign;
+    Times[STAGE_MAXSPEED] = Times[0] + dt01;
+    dt23 = setspeed / Max.accel;
+    dx23 = setspeed * dt23 / 2.;
+    // calculate dx12 and dt12
+    double dx12 = Dx - dx01 - dx23;
+    if(dx12 < -coord_tolerance){
+        DBG("Oops, WTF dx12=%g?", dx12);
+        return FALSE;
+    }
+    double dt12 = dx12 / setspeed;
+    p = &Params[STAGE_DECEL];
+    p->accel = -sign * Max.accel;
+    p->speed = sign * setspeed;
+    p->coord = Params[STAGE_MAXSPEED].coord + sign * dx12;
+    Times[STAGE_DECEL] = Times[STAGE_MAXSPEED] + dt12;
+    p = &Params[STAGE_STOPPED];
+    p->accel = 0.; p->speed = 0.; p->coord = x->coord;
+    Times[STAGE_STOPPED] = Times[STAGE_DECEL] + dt23;
+    for(int i = 0; i < 4; ++i)
+        DBG("%d: t=%g, coord=%g, speed=%g, accel=%g", i,
+            Times[i], Params[i].coord, Params[i].speed, Params[i].accel);
+    state = ST_MOVE;
+    movingstage = STAGE_ACCEL;
+    return TRUE;
+}
+
+static movestate_t proc(moveparam_t *next, double t){
+    if(state == ST_STOP) goto ret;
+    for(movingstage_t s = STAGE_STOPPED; s >= 0; --s){
+        if(Times[s] <= t){ // check time for current stage
+            movingstage = s;
+            break;
+        }
+    }
+    if(movingstage == STAGE_STOPPED){
+        curparams.coord = Params[STAGE_STOPPED].coord;
+        emstop(t);
+        goto ret;
+    }
+    // calculate current parameters
+    double dt = t - Times[movingstage];
+    double a = Params[movingstage].accel;
+    double v0 = Params[movingstage].speed;
+    double x0 = Params[movingstage].coord;
+    curparams.accel = a;
+    curparams.speed = v0 + a * dt;
+    curparams.coord = x0 + v0 * dt + a * dt * dt / 2.;
+ret:
+    if(next) *next = curparams;
+    return state;
+}
+
+static movestate_t getst(moveparam_t *cur){
+    if(cur) *cur = curparams;
+    return state;
+}
+
+static double gettstop(){
+    return Times[STAGE_STOPPED];
+}
+
+movemodel_t trapez = {
+    .init_limits = initlims,
+    .stop = stop,
+    .emergency_stop = emstop,
+    .get_state = getst,
+    .calculate = calc,
+    .proc_move = proc,
+    .stoppenanotime = gettstop,
+};

LibSidServo/ramp.h

@@ -0,0 +1,23 @@
+/*
+ * This file is part of the moving_model 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 "movingmodel.h"
+
+extern movemodel_t trapez;

LibSidServo/serial.c

@@ -21,16 +21,17 @@
 #include <fcntl.h>
 #include <math.h>
 #include <pthread.h>
+#include <signal.h>
 #include <stdint.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
 #include <sys/ioctl.h>
 #include <sys/stat.h>
-#include <time.h>
 #include <unistd.h>
 
-#include "dbg.h"
+#include "main.h"
+#include "movingmodel.h"
 #include "serial.h"
 
 // serial devices FD
@@ -55,45 +56,6 @@ typedef struct __attribute__((packed)){
     uint8_t CRC[4];
 } enc_t;
 
-/**
- * @brief dtime - monotonic time from first run
- * @return
- */
-double dtime(){
-    struct timespec start_time = {0}, cur_time;
-    if(start_time.tv_sec == 0 && start_time.tv_nsec == 0){
-        clock_gettime(CLOCK_MONOTONIC, &start_time);
-    }
-    clock_gettime(CLOCK_MONOTONIC, &cur_time);
-    return ((double)(cur_time.tv_sec - start_time.tv_sec) +
-                     (cur_time.tv_nsec - start_time.tv_nsec) * 1e-9);
-}
-#if 0
-double dtime(){
-    double t;
-    struct timeval tv;
-    gettimeofday(&tv, NULL);
-    t = tv.tv_sec + ((double)tv.tv_usec)/1e6;
-    return t;
-}
-#endif
-#if 0
-double tv2d(struct timeval *tv){
-    if(!tv) return 0.;
-    double t = tv->tv_sec + ((double)tv->tv_usec) / 1e6;
-    return t;
-}
-#endif
-#if 0
-// init start time
-static void gttime(){
-    struct timeval tv;
-    gettimeofday(&tv, NULL);
-    tv_sec_got = tv.tv_sec;
-    tv_usec_got = tv.tv_usec;
-}
-#endif
-
 // calculate current X/Y speeds
 static void getXspeed(double t){
     mountdata.encXspeed.val = (mountdata.encXposition.val - lastXenc.val) / (t - lastXenc.t);
@@ -170,7 +132,7 @@ static int getencval(int fd, double *val, double *t){
     if(fd < 0) return FALSE;
     char buf[128];
     int got = 0, Lmax = 127;
-    double t0 = dtime();
+    double t0 = nanotime();
     do{
         fd_set rfds;
         FD_ZERO(&rfds);
@@ -189,7 +151,7 @@ static int getencval(int fd, double *val, double *t){
             buf[got] = 0;
         } else continue;
         if(strchr(buf, '\n')) break;
-    }while(Lmax && dtime() - t0 < Conf.EncoderReqInterval);
+    }while(Lmax && nanotime() - t0 < Conf.EncoderReqInterval);
     if(got == 0) return 0; // WTF?
     char *estr = strrchr(buf, '\n');
     if(!estr) return 0;
@@ -277,7 +239,7 @@ static void *encoderthread1(void _U_ *u){
             if((uint8_t)b == ENC_MAGICK){
 //                DBG("Got magic -> start filling packet");
                 databuf[wridx++] = (uint8_t) b;
-                t = dtime();
+                t = nanotime();
             }
             continue;
         }else databuf[wridx++] = (uint8_t) b;
@@ -298,13 +260,13 @@ static void *encoderthread2(void _U_ *u){
     if(Conf.SepEncoder != 2) return NULL;
     DBG("Thread started");
     int errctr = 0;
-    double t0 = dtime();
+    double t0 = nanotime();
-    const char *req = "next\n";
+    const char *req = "\n";
     int need2ask = 0; // need or not to ask encoder for new data
     while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR){
         if(need2ask){
-            if(5 != write(encfd[0], req, 5)) { ++errctr; continue; }
+            if(1 != write(encfd[0], req, 1)) { ++errctr; continue; }
-            else if(5 != write(encfd[1], req, 5)) { ++errctr; continue; }
+            else if(1 != write(encfd[1], req, 1)) { ++errctr; continue; }
         }
         double v, t;
         if(getencval(encfd[0], &v, &t)){
@@ -329,9 +291,9 @@ static void *encoderthread2(void _U_ *u){
             else need2ask = 1;
             continue;
         }
-        while(dtime() - t0 < Conf.EncoderReqInterval){ usleep(10); }
+        while(nanotime() - t0 < Conf.EncoderReqInterval){ usleep(50); }
-        //DBG("DT=%g (RI=%g)", dtime()-t0, Conf.EncoderReqInterval);
+        //DBG("DT=%g (RI=%g)", nanotime()-t0, Conf.EncoderReqInterval);
-        t0 = dtime();
+        t0 = nanotime();
     }
     DBG("ERRCTR=%d", errctr);
     for(int i = 0; i < 2; ++i){
@@ -364,20 +326,25 @@ static void *mountthread(void _U_ *u){
     int errctr = 0;
     uint8_t buf[2*sizeof(SSstat)];
     SSstat *status = (SSstat*) buf;
+    if(Conf.RunModel)
+    while(1){
+        pthread_mutex_lock(&datamutex);
+        // now change data
+        getModData(&mountdata);
+        pthread_mutex_unlock(&datamutex);
+        double t0 = nanotime();
+        while(nanotime() - t0 < Conf.MountReqInterval) usleep(500);
+        t0 = nanotime();
+    }
     // data to get
     data_t d = {.buf = buf, .maxlen = sizeof(buf)};
     // cmd to send
     data_t *cmd_getstat = cmd2dat(CMD_GETSTAT);
     if(!cmd_getstat) goto failed;
-    double t0 = dtime();
-/*
-#ifdef EBUG
-    double t00 = t0;
-#endif
-*/
     while(mntfd > -1 && errctr < MAX_ERR_CTR){
         // read data to status
-        double tgot = dtime();
+        double tgot = nanotime(), t0 = tgot;
+        DBG("tgot=%g", tgot);
         if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){
 #ifdef EBUG
             DBG("Can't read SSstat, need %zd got %zd bytes", sizeof(SSstat), d.len);
@@ -396,10 +363,8 @@ static void *mountthread(void _U_ *u){
         SSconvstat(status, &mountdata, tgot);
         pthread_mutex_unlock(&datamutex);
         // allow writing & getters
-        //DBG("t0=%g, tnow=%g", t0-t00, dtime()-t00);
+        while(nanotime() - t0 < Conf.MountReqInterval) usleep(500);
-        if(dtime() - t0 >= Conf.MountReqInterval) usleep(50);
+        t0 = nanotime();
-        while(dtime() - t0 < Conf.MountReqInterval);
-        t0 = dtime();
     }
     data_free(&cmd_getstat);
 failed:
@@ -435,6 +400,7 @@ static int ttyopen(const char *path, speed_t speed){
 
 // return FALSE if failed
 int openEncoder(){
+    if(Conf.RunModel) return TRUE;
     if(!Conf.SepEncoder) return FALSE; // try to open separate encoder when it's absent
     if(Conf.SepEncoder == 1){ // only one device
         DBG("One device");
@@ -442,7 +408,7 @@ int openEncoder(){
         encfd[0] = ttyopen(Conf.EncoderDevPath, (speed_t) Conf.EncoderDevSpeed);
         if(encfd[0] < 0) return FALSE;
         encRtmout.tv_sec = 0;
-        encRtmout.tv_usec = 200000000 / Conf.EncoderDevSpeed; // 20 bytes
+        encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed; // 10 bytes
         if(pthread_create(&encthread, NULL, encoderthread1, NULL)){
             close(encfd[0]);
             encfd[0] = -1;
@@ -472,6 +438,7 @@ int openEncoder(){
 
 // return FALSE if failed
 int openMount(){
+    if(Conf.RunModel) return TRUE;
     if(mntfd > -1) close(mntfd);
     DBG("Open mount %s @ %d", Conf.MountDevPath, Conf.MountDevSpeed);
     mntfd = ttyopen(Conf.MountDevPath, (speed_t) Conf.MountDevSpeed);
@@ -487,7 +454,7 @@ int openMount(){
         DBG("got %zd", l);
     }while(1);*/
     mntRtmout.tv_sec = 0;
-    mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes
+    mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes * 10bits / speed
     if(pthread_create(&mntthread, NULL, mountthread, NULL)){
         DBG("Can't create thread");
         close(mntfd);
@@ -500,21 +467,22 @@ int openMount(){
 
 // close all opened serial devices and quit threads
 void closeSerial(){
+    if(Conf.RunModel) return;
     if(mntfd > -1){
-        DBG("Kill mount thread");
+        DBG("Cancel mount thread");
         pthread_cancel(mntthread);
         DBG("join mount thread");
         pthread_join(mntthread, NULL);
-        DBG("close fd");
+        DBG("close mount fd");
         close(mntfd);
         mntfd = -1;
     }
     if(encfd[0] > -1){
-        DBG("Kill encoder thread");
+        DBG("Cancel encoder thread");
         pthread_cancel(encthread);
         DBG("join encoder thread");
         pthread_join(encthread, NULL);
-        DBG("close fd");
+        DBG("close encoder's fd");
         close(encfd[0]);
         encfd[0] = -1;
         if(Conf.SepEncoder == 2){
@@ -548,7 +516,7 @@ static int wr(const data_t *out, data_t *in, int needeol){
             DBG("written bytes not equal to need");
             return FALSE;
         }
-        //DBG("Send to mount %zd bytes: %s", out->len, out->buf);
+        DBG("Send to mount %zd bytes: %s", out->len, out->buf);
         if(needeol){
             int g = write(mntfd, "\r", 1); // add EOL
             (void) g;
@@ -563,8 +531,9 @@ static int wr(const data_t *out, data_t *in, int needeol){
         if(b < 0) break; // nothing to read -> go out
         in->buf[in->len++] = (uint8_t) b;
     }
+    DBG("got %zd bytes", in->len);
     //DBG("Clear trashing input");
-    while(getmntbyte() > -1);
+    //while(getmntbyte() > -1);
     return TRUE;
 }
 
@@ -575,6 +544,7 @@ static int wr(const data_t *out, data_t *in, int needeol){
  * @return FALSE if failed
  */
 int MountWriteRead(const data_t *out, data_t *in){
+    if(Conf.RunModel) return -1;
     pthread_mutex_lock(&mntmutex);
     int ret = wr(out, in, 1);
     pthread_mutex_unlock(&mntmutex);
@@ -582,6 +552,7 @@ int MountWriteRead(const data_t *out, data_t *in){
 }
 // send binary data - without EOL
 int MountWriteReadRaw(const data_t *out, data_t *in){
+    if(Conf.RunModel) return -1;
     pthread_mutex_lock(&mntmutex);
     int ret = wr(out, in, 0);
     pthread_mutex_unlock(&mntmutex);
@@ -605,6 +576,7 @@ static void loglcmd(SSlcmd *c){
 
 // send short/long binary command; return FALSE if failed
 static int bincmd(uint8_t *cmd, int len){
+    if(Conf.RunModel) return FALSE;
     static data_t *dscmd = NULL, *dlcmd = NULL;
     if(!dscmd) dscmd = cmd2dat(CMD_SHORTCMD);
     if(!dlcmd) dlcmd = cmd2dat(CMD_LONGCMD);
@@ -641,6 +613,7 @@ rtn:
     return ret;
 }
 
+// short, long and config text-binary commands
 // return TRUE if OK
 int cmdS(SSscmd *cmd){
     return bincmd((uint8_t *)cmd, sizeof(SSscmd));
@@ -650,6 +623,7 @@ int cmdL(SSlcmd *cmd){
 }
 // rw == 1 to write, 0 to read
 int cmdC(SSconfig *conf, int rw){
+    if(Conf.RunModel) return FALSE;
     static data_t *wcmd = NULL, *rcmd = NULL;
     int ret = FALSE;
     // dummy buffer to clear trash in input

LibSidServo/serial.h

@@ -28,7 +28,6 @@
 // max error counter (when read() returns -1)
 #define MAX_ERR_CTR (100)
 
-double dtime();
 data_t *cmd2dat(const char *cmd);
 void data_free(data_t **x);
 int openEncoder();

LibSidServo/sidservo.h

@@ -16,6 +16,11 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+/*
+ * This file contains all need for external usage
+ */
+
+
 #pragma once
 
 #ifdef __cplusplus
@@ -30,6 +35,10 @@ extern "C"
 // max speeds (rad/s): xs=10 deg/s, ys=8 deg/s
 #define MCC_MAX_X_SPEED         (0.174533)
 #define MCC_MAX_Y_SPEED         (0.139626)
+// accelerations by both axis (for model); TODO: move speeds/accelerations into config?
+// xa=12.6 deg/s^2, ya= 9.5 deg/s^2
+#define MCC_X_ACCELERATION      (0.219911)
+#define MCC_Y_ACCELERATION      (0.165806)
 
 // max speed interval, seconds
 #define MCC_CONF_MAX_SPEEDINT   (2.)
@@ -57,6 +66,7 @@ typedef struct{
     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
 } conf_t;
 
 // coordinates/speeds in degrees or d/s: X, Y
@@ -224,7 +234,6 @@ typedef struct{
 
 extern mount_t Mount;
 
-
 #ifdef __cplusplus
 }
 #endif

LibSidServo/ssii.c

@@ -19,8 +19,9 @@
 #include <ctype.h>
 #include <inttypes.h>
 #include <string.h>
+#include <unistd.h>
 
-#include "dbg.h"
+#include "main.h"
 #include "serial.h"
 #include "ssii.h"
 
@@ -65,13 +66,6 @@ static void ChkStopped(const SSstat *s, mountdata_t *m){
  */
 void SSconvstat(const SSstat *s, mountdata_t *m, double t){
     if(!s || !m) return;
-/*
-#ifdef EBUG
-    static double t0 = -1.;
-    if(t0 < 0.) t0 = dtime();
-#endif
-    DBG("Convert, t=%g", dtime()-t0);
-*/
     m->motXposition.val = X_MOT2RAD(s->Xmot);
     m->motYposition.val = Y_MOT2RAD(s->Ymot);
     ChkStopped(s, m);
@@ -82,9 +76,6 @@ void SSconvstat(const SSstat *s, mountdata_t *m, double t){
         m->encYposition.val = Y_ENC2RAD(s->Yenc);
         m->encXposition.t = m->encYposition.t = t;
     }
-    //m->lastmotposition.X = X_MOT2RAD(s->XLast);
-    //m->lastmotposition.Y = Y_MOT2RAD(s->YLast);
-    //m->lastmotposition.msrtime = *tdat;
     m->keypad = s->keypad;
     m->extradata.ExtraBits = s->ExtraBits;
     m->extradata.ain0 = s->ain0;
@@ -170,7 +161,9 @@ int SSstop(int emerg){
     const char *cmdx = (emerg) ? CMD_EMSTOPX : CMD_STOPX;
     const char *cmdy = (emerg) ? CMD_EMSTOPY : CMD_STOPY;
     for(; i < 10; ++i){
+        DBG("t1: %g", nanotime());
         if(!SStextcmd(cmdx, NULL)) continue;
+        DBG("t2: %g", nanotime());
         if(SStextcmd(cmdy, NULL)) break;
     }
     if(i == 10) return FALSE;
@@ -180,12 +173,19 @@ int SSstop(int emerg){
 // update motors' positions due to encoders'
 mcc_errcodes_t updateMotorPos(){
     mountdata_t md = {0};
-    double t0 = dtime(), t = 0.;
+    double t0 = nanotime(), t = 0.;
     DBG("start @ %g", t0);
     do{
-        t = dtime();
+        t = nanotime();
         if(MCC_E_OK == getMD(&md)){
-            DBG("got");
+            if(md.encXposition.t == 0 || md.encYposition.t == 0){
+                DBG("Just started, t-t0 = %g!", t - t0);
+                sleep(1);
+                DBG("t-t0 = %g", nanotime() - t0);
+                //usleep(10000);
+                continue;
+            }
+            DBG("got; t pos x/y: %g/%g; tnow: %g", md.encXposition.t, md.encYposition.t, t);
             if(fabs(md.encXposition.t - t) < 0.1 && fabs(md.encYposition.t - t) < 0.1){
                 DBG("FIX motors position to encoders");
                 int32_t Xpos = X_RAD2MOT(md.encXposition.val), Ypos = Y_RAD2MOT(md.encYposition.val);

LibSidServo/ssii.h

@@ -16,6 +16,10 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+/*
+ * This file contains stuff for sidereal-servo specific protocol
+ */
+
 #pragma once
 
 #include <math.h>
@@ -171,16 +175,30 @@
 // amount of consequent same coordinates to detect stop
 #define MOTOR_STOPPED_CNT       (20)
 
+// TODO: take it from settings?
 // steps per revolution (SSI - x4 - for SSI)
-// 13312000 / 4 = 3328000
 #define X_MOT_STEPSPERREV_SSI   (13312000.)
-//#define X_MOT_STEPSPERREV   (3325952.)
+// 13312000 / 4 = 3328000
 #define X_MOT_STEPSPERREV   (3328000.)
-// 17578668 / 4 = 4394667
 #define Y_MOT_STEPSPERREV_SSI (17578668.)
-//#define Y_MOT_STEPSPERREV   (4394960.)
+// 17578668 / 4 = 4394667
 #define Y_MOT_STEPSPERREV   (4394667.)
 
+// encoder per revolution
+#define X_ENC_STEPSPERREV   (67108864.)
+#define Y_ENC_STEPSPERREV   (67108864.)
+// encoder zero position
+#define X_ENC_ZERO          (61245239)
+#define Y_ENC_ZERO          (36999830)
+// encoder reversed (no: +1)
+#define X_ENC_SIGN          (-1.)
+#define Y_ENC_SIGN          (-1.)
+// encoder position to radians and back
+#define X_ENC2RAD(n)    ang2half(X_ENC_SIGN * 2.*M_PI * ((double)(n-X_ENC_ZERO)) / X_ENC_STEPSPERREV)
+#define Y_ENC2RAD(n)    ang2half(Y_ENC_SIGN * 2.*M_PI * ((double)(n-Y_ENC_ZERO)) / Y_ENC_STEPSPERREV)
+#define X_RAD2ENC(r)    ((uint32_t)((r) / 2./M_PI * X_ENC_STEPSPERREV))
+#define Y_RAD2ENC(r)    ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV))
+
 // convert angle in radians to +-pi
 static inline double ang2half(double ang){
     if(ang < -M_PI) ang += 2.*M_PI;
@@ -214,21 +232,6 @@ static inline double ang2full(double ang){
 #define ADDER2S(a)  ((a) / SITECH_LOOP_FREQUENCY)
 #define S2ADDER(s)  ((s) * SITECH_LOOP_FREQUENCY)
 
-// encoder per revolution
-#define X_ENC_STEPSPERREV   (67108864.)
-#define Y_ENC_STEPSPERREV   (67108864.)
-// encoder zero position
-#define X_ENC_ZERO          (61245239)
-#define Y_ENC_ZERO          (36999830)
-// encoder reversed (no: +1)
-#define X_ENC_SIGN          (-1.)
-#define Y_ENC_SIGN          (-1.)
-// encoder position to radians and back
-#define X_ENC2RAD(n)    ang2half(X_ENC_SIGN * 2.*M_PI * ((double)(n-X_ENC_ZERO)) / X_ENC_STEPSPERREV)
-#define Y_ENC2RAD(n)    ang2half(Y_ENC_SIGN * 2.*M_PI * ((double)(n-Y_ENC_ZERO)) / Y_ENC_STEPSPERREV)
-#define X_RAD2ENC(r)    ((uint32_t)((r) / 2./M_PI * X_ENC_STEPSPERREV))
-#define Y_RAD2ENC(r)    ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV))
-
 // encoder's tolerance (ticks)
 #define YencTOL    (25.)
 #define XencTOL    (25.)
@@ -334,5 +337,4 @@ int SSrawcmd(const char *cmd, data_t *answer);
 int SSgetint(const char *cmd, int64_t *ans);
 int SSsetterI(const char *cmd, int32_t ival);
 int SSstop(int emerg);
-int SSshortCmd(SSscmd *cmd);
 mcc_errcodes_t updateMotorPos();