last week

LibSidServo/examples/dumpswing.c

@@ -55,7 +55,7 @@ static sl_option_t cmdlnopts[] = {
     {"coordsfile",  NEED_ARG,   NULL,   'o',    arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
     {"axis",        NEED_ARG,   NULL,   'a',    arg_string, APTR(&G.axis),      "axis to move (X or Y)"},
     {"period",      NEED_ARG,   NULL,   'p',    arg_double, APTR(&G.period),    "swinging period (could be not reached if amplitude is too small) - not more than 900s (default: 1)"},
-    {"amplitude",   NEED_ARG,   NULL,   'A',    arg_double, APTR(&G.amplitude), "max amplitude (could be not reaced if period is too small) - not more than 45deg (default: 5)"},
+    {"amplitude",   NEED_ARG,   NULL,   'A',    arg_double, APTR(&G.amplitude), "max amplitude (could be not reached if period is too small): [-45:45]deg (default: 5)"},
     {"nswings",     NEED_ARG,   NULL,   'N',    arg_int,    APTR(&G.Nswings),   "amount of swing periods (default: 10)"},
     {"conffile",    NEED_ARG,   NULL,   'C',    arg_int,    APTR(&G.conffile),  "configuration file name"},
     end_option
@@ -114,7 +114,8 @@ int main(int argc, char **argv){
             ERRX("Can't open %s", G.coordsoutput);
     }else fcoords = stdout;
     if(G.Ncycles < 7) ERRX("Ncycles should be >7");
-    if(G.amplitude < 0.01 || G.amplitude > 45.)
+    double absamp = fabs(G.amplitude);
+    if(absamp < 0.01 || absamp > 45.)
         ERRX("Amplitude should be from 0.01 to 45 degrees");
     if(G.period < 0.1 || G.period > 900.)
         ERRX("Period should be from 0.1 to 900s");

LibSidServo/movingmodel.h

@@ -22,8 +22,8 @@
 #include "sidservo.h"
 
 // tolerance, time ticks
-#define COORD_TOLERANCE_DEFAULT (1e-6)
+#define COORD_TOLERANCE_DEFAULT (1e-8)
-#define COORD_TOLERANCE_MIN     (1e-8)
+#define COORD_TOLERANCE_MIN     (1e-12)
 #define COORD_TOLERANCE_MAX     (10.)
 #define TIME_TICK_DEFAULT       (0.0001)
 #define TIME_TICK_MIN           (1e-9)

LibSidServo/ramp.c

@@ -94,11 +94,6 @@ static int calc(movemodel_t *m, moveparam_t *x, double t){
     double dt0s = curspeed / m->Max.accel; // time of stopping phase
     double dx0s = curspeed * dt0s / 2.; // distance
     DBG("dt0s=%g, dx0s=%g, curspeed=%g", dt0s, dx0s, curspeed);
-    // TODO: fix this! Target should stop and after thar reach given coordinate!!!
-    if(dx0s - Dx > coord_tolerance){
-        DBG("distance too short");
-        goto ret;
-    }
     if(fabs(Dx - dx0s) < coord_tolerance){ // just stop and we'll be on target
         DBG("Distance good to just stop");
         pthread_mutex_unlock(&m->mutex);
@@ -106,17 +101,58 @@ static int calc(movemodel_t *m, moveparam_t *x, double t){
         ret = TRUE;
         goto ret;
     }
-    if(m->curparams.speed * sign < 0. || m->state == ST_STOP){ // we should change speed sign
+    if(m->curparams.speed * sign < 0. || m->state == ST_STOP || (dx0s > Dx + coord_tolerance)){ // we should change speed sign
-        // after stop we will have full profile
+        DBG("SIGN of speed should be changed!");
-        double dxs3 = Dx - dx0s;
+        double sign_current = (m->curparams.speed >= 0.) ? 1. : -1.;
-        double newspeed = sqrt(m->Max.accel * dxs3);
+        if (m->state == ST_STOP) {
-        if(newspeed < setspeed) setspeed = newspeed; // we can't reach user speed
+            // Already stopped
-        DBG("dxs3=%g, setspeed=%g", dxs3, setspeed);
+            dx0s = 0.;
-        dt01 = fabs(sign*setspeed - m->curparams.speed) / m->Max.accel;
+            sign_current = 0.;
-        m->Params[0].accel = sign * m->Max.accel;
+        }
-        if(m->state == ST_STOP) dx01 = setspeed * dt01 / 2.;
+        double stop_coord = m->curparams.coord + sign_current * dx0s;
-        else dx01 = dt01 * (dt01 / 2. * m->Max.accel - curspeed);
+        double Dx_after = fabs(x->coord - stop_coord);
-        DBG("dx01=%g, dt01=%g", dx01, dt01);
+        double sign_after = (x->coord > stop_coord) ? 1. : -1.;
+
+        // Calculate new max speed for reverse movement
+        double setspeed = sqrt(m->Max.accel * Dx_after);
+        if (setspeed > x->speed) setspeed = x->speed;
+        if (setspeed > m->Max.speed) setspeed = m->Max.speed;
+        if (setspeed < m->Min.speed) {
+            DBG("New speed (%g) too small (<%g)", setspeed, m->Min.speed);
+            goto ret;
+        }
+
+        double t_acc = setspeed / m->Max.accel;
+
+        // Stage 0: Stop current movement (if moving)
+        m->Times[STAGE_ACCEL] = t;
+        m->Params[STAGE_ACCEL].coord = m->curparams.coord;
+        m->Params[STAGE_ACCEL].speed = m->curparams.speed;
+        if (m->state != ST_STOP) {
+            m->Params[STAGE_ACCEL].accel = -sign_current * m->Max.accel;
+            m->Times[STAGE_MAXSPEED] = t + dt0s;
+        } else {
+            m->Params[STAGE_ACCEL].accel = 0.;
+            m->Times[STAGE_MAXSPEED] = t;
+        }
+
+        // Stage 1: Accelerate in opposite direction
+        m->Params[STAGE_MAXSPEED].coord = stop_coord;
+        m->Params[STAGE_MAXSPEED].speed = 0.;
+        m->Params[STAGE_MAXSPEED].accel = sign_after * m->Max.accel;
+        m->Times[STAGE_DECEL] = m->Times[STAGE_MAXSPEED] + t_acc;
+        m->Params[STAGE_DECEL].coord = stop_coord + sign_after * (0.5 * m->Max.accel * t_acc * t_acc);
+        m->Params[STAGE_DECEL].speed = sign_after * setspeed;
+        m->Params[STAGE_DECEL].accel = -sign_after * m->Max.accel;
+
+        // Stage 2: Decelerate to stop at target
+        m->Times[STAGE_STOPPED] = m->Times[STAGE_DECEL] + t_acc;
+        m->Params[STAGE_STOPPED].coord = x->coord;
+        m->Params[STAGE_STOPPED].speed = 0.;
+        m->Params[STAGE_STOPPED].accel = 0.;
+
+        ret = TRUE;
+        goto ret;
     }else{ // increase or decrease speed without stopping phase
         dt01 = fabs(sign*setspeed - m->curparams.speed) / m->Max.accel;
         double a = sign * m->Max.accel;
@@ -128,7 +164,7 @@ static int calc(movemodel_t *m, moveparam_t *x, double t){
         if(dx01 + dx23 > Dx){ // calculate max speed
             setspeed = sqrt(m->Max.accel * Dx - curspeed * curspeed / 2.);
             if(setspeed < curspeed){
-                setspeed = m->curparams.speed;
+                setspeed = curspeed;
                 dt01 = 0.; dx01 = 0.;
                 m->Params[0].accel = 0.;
             }else{
@@ -164,13 +200,15 @@ static int calc(movemodel_t *m, moveparam_t *x, double t){
     p = &m->Params[STAGE_STOPPED];
     p->accel = 0.; p->speed = 0.; p->coord = x->coord;
     m->Times[STAGE_STOPPED] = m->Times[STAGE_DECEL] + dt23;
+    ret = TRUE;
+ret:
+    if(ret){
+        m->state = ST_MOVE;
+        m->movingstage = STAGE_ACCEL;
         for(int i = 0; i < 4; ++i)
             DBG("%d: t=%g, coord=%g, speed=%g, accel=%g", i,
                 m->Times[i], m->Params[i].coord, m->Params[i].speed, m->Params[i].accel);
-    m->state = ST_MOVE;
+    }
-    m->movingstage = STAGE_ACCEL;
-    ret = TRUE;
-ret:
     pthread_mutex_unlock(&m->mutex);
     return ret;
 }

LibSidServo/sidservo.h

@@ -32,6 +32,11 @@ extern "C"
 #include <stdint.h>
 #include <sys/time.h>
 
+// acceptable position error - 0.1''
+#define MCC_POSITION_ERROR      (5e-7)
+// acceptable disagreement between motor and axe encoders - 2''
+#define MCC_ENCODERS_ERROR      (1e-7)
+
 // 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)

LibSidServo/ssii.c

@@ -189,16 +189,24 @@ mcc_errcodes_t updateMotorPos(){
                 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){
+            mcc_errcodes_t OK = MCC_E_OK;
-                DBG("FIX motors position to encoders");
+            if(fabs(md.motXposition.val - md.encXposition.val) > MCC_ENCODERS_ERROR && md.Xstatus == MNT_STOPPED){
-                int32_t Xpos = X_RAD2MOT(md.encXposition.val), Ypos = Y_RAD2MOT(md.encYposition.val);
+                DBG("NEED to sync X: motors=%g, axes=%g", md.motXposition.val, md.encXposition.val);
-                if(SSsetterI(CMD_MOTXSET, Xpos) && SSsetterI(CMD_MOTYSET, Ypos)){
+                if(!SSsetterI(CMD_MOTXSET, X_RAD2MOT(md.encXposition.val))){
-                    DBG("All OK, Dt=%g", nanotime() - t0);
+                    DBG("Xpos sync failed!");
-                    return MCC_E_OK;
+                    OK = MCC_E_FAILED;
+                }else DBG("Xpos sync OK, Dt=%g", nanotime() - t0);
             }
-            }else{
+            if(fabs(md.motYposition.val - md.encYposition.val) > MCC_ENCODERS_ERROR && md.Xstatus == MNT_STOPPED){
-                DBG("on position");
+                DBG("NEED to sync Y: motors=%g, axes=%g", md.motYposition.val, md.encYposition.val);
-                return MCC_E_OK;
+                if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){
+                    DBG("Ypos sync failed!");
+                    OK = MCC_E_FAILED;
+                }else DBG("Ypos sync OK, Dt=%g", nanotime() - t0);
+            }
+            if(MCC_E_OK == OK){
+                DBG("Encoders synced");
+                return OK;
             }
         }
         DBG("NO DATA; dt = %g", t - t0);