From 219cec60556d608565484a638c9328ac5e54c7d8 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Mon, 4 Aug 2025 09:02:22 +0300 Subject: [PATCH] last week --- LibSidServo/examples/dumpswing.c | 5 +- LibSidServo/movingmodel.h | 4 +- LibSidServo/ramp.c | 82 +++++++++++++++++++++++--------- LibSidServo/sidservo.h | 5 ++ LibSidServo/ssii.c | 28 +++++++---- 5 files changed, 88 insertions(+), 36 deletions(-) diff --git a/LibSidServo/examples/dumpswing.c b/LibSidServo/examples/dumpswing.c index c023b11..2facc3d 100644 --- a/LibSidServo/examples/dumpswing.c +++ b/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"); diff --git a/LibSidServo/movingmodel.h b/LibSidServo/movingmodel.h index 1c70982..36e93c3 100644 --- a/LibSidServo/movingmodel.h +++ b/LibSidServo/movingmodel.h @@ -22,8 +22,8 @@ #include "sidservo.h" // tolerance, time ticks -#define COORD_TOLERANCE_DEFAULT (1e-6) -#define COORD_TOLERANCE_MIN (1e-8) +#define COORD_TOLERANCE_DEFAULT (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) diff --git a/LibSidServo/ramp.c b/LibSidServo/ramp.c index f4b74b4..5859394 100644 --- a/LibSidServo/ramp.c +++ b/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 - // after stop we will have full profile - double dxs3 = Dx - dx0s; - double newspeed = sqrt(m->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 - m->curparams.speed) / m->Max.accel; - m->Params[0].accel = sign * m->Max.accel; - if(m->state == ST_STOP) dx01 = setspeed * dt01 / 2.; - else dx01 = dt01 * (dt01 / 2. * m->Max.accel - curspeed); - DBG("dx01=%g, dt01=%g", dx01, dt01); + if(m->curparams.speed * sign < 0. || m->state == ST_STOP || (dx0s > Dx + coord_tolerance)){ // we should change speed sign + DBG("SIGN of speed should be changed!"); + double sign_current = (m->curparams.speed >= 0.) ? 1. : -1.; + if (m->state == ST_STOP) { + // Already stopped + dx0s = 0.; + sign_current = 0.; + } + double stop_coord = m->curparams.coord + sign_current * dx0s; + double Dx_after = fabs(x->coord - stop_coord); + 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; - 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: + 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); + } pthread_mutex_unlock(&m->mutex); return ret; } diff --git a/LibSidServo/sidservo.h b/LibSidServo/sidservo.h index 0ae7fd4..f8ab95c 100644 --- a/LibSidServo/sidservo.h +++ b/LibSidServo/sidservo.h @@ -32,6 +32,11 @@ extern "C" #include #include +// 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) diff --git a/LibSidServo/ssii.c b/LibSidServo/ssii.c index ca293b8..b76faff 100644 --- a/LibSidServo/ssii.c +++ b/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){ - DBG("FIX motors position to encoders"); - int32_t Xpos = X_RAD2MOT(md.encXposition.val), Ypos = Y_RAD2MOT(md.encYposition.val); - if(SSsetterI(CMD_MOTXSET, Xpos) && SSsetterI(CMD_MOTYSET, Ypos)){ - DBG("All OK, Dt=%g", nanotime() - t0); - return MCC_E_OK; - } - }else{ - DBG("on position"); - return MCC_E_OK; + mcc_errcodes_t OK = MCC_E_OK; + if(fabs(md.motXposition.val - md.encXposition.val) > MCC_ENCODERS_ERROR && md.Xstatus == MNT_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){ + 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!"); + 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);