diff --git a/LibSidServo/examples/scmd_traectory.c b/LibSidServo/examples/scmd_traectory.c index 10e73f5..5acf339 100644 --- a/LibSidServo/examples/scmd_traectory.c +++ b/LibSidServo/examples/scmd_traectory.c @@ -30,6 +30,7 @@ typedef struct{ int help; + int dumpconf; int Ncycles; // n cycles to wait stop double reqint; // requests interval (seconds) double Xmax; // maximal X to stop @@ -69,6 +70,7 @@ static sl_option_t cmdlnopts[] = { {"y0", NEED_ARG, NULL, '1', arg_double, APTR(&G.Y0), "starting Y-coordinate of traectory (default: 10 degrees)"}, {"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"}, {"errlog", NEED_ARG, NULL, 'e', arg_string, APTR(&G.errlog), "file with errors log"}, + {"dumpconf", NO_ARGS, NULL, 'D', arg_int, APTR(&G.dumpconf), "dump current configuration"}, end_option }; @@ -150,7 +152,7 @@ int main(int argc, char **argv){ ERRX("Can't open %s", G.coordsoutput); }else fcoords = stdout; conf_t *Config = readServoConf(G.conffile); - if(!Config){ + if(!Config || G.dumpconf){ dumpConf(); return 1; } diff --git a/LibSidServo/examples/servo.conf b/LibSidServo/examples/servo.conf index dfe0a25..220926e 100644 --- a/LibSidServo/examples/servo.conf +++ b/LibSidServo/examples/servo.conf @@ -1,9 +1,25 @@ +Current configuration: MountDevPath=/dev/ttyUSB0 MountDevSpeed=19200 +EncoderDevPath=(null) +EncoderDevSpeed=1000000 +MountReqInterval=0.1 +EncoderReqInterval=0.001 +SepEncoder=2 EncoderXDevPath=/dev/encoder_X0 EncoderYDevPath=/dev/encoder_Y0 -MountReqInterval=0.05 -SepEncoder=2 -EncoderReqInterval=0.001 -EncoderDevSpeed=1000000 EncoderSpeedInterval=0.05 +RunModel=1 +XPIDCP=0.8 +XPIDCI=0.0 +XPIDCD=0.0 +YPIDCP=0.5 +YPIDCI=0.0 +YPIDCD=0.0 +XPIDVP=0.2 +XPIDVI=0.1 +XPIDVD=0.0 +YPIDVP=0.2 +YPIDVI=0.1 +YPIDVD=0.0 + diff --git a/LibSidServo/mainconf.conf b/LibSidServo/mainconf.conf deleted file mode 100644 index dd4d0a0..0000000 --- a/LibSidServo/mainconf.conf +++ /dev/null @@ -1,7 +0,0 @@ -# configuration file for SSII driven equatorial mount -verbose = 5 -mountpath = /dev/ttyS1 -mountspeed = 19200 -encoderpath = /dev/ttyUSB0 -encoderspeed = 153000 - diff --git a/LibSidServo/movingmodel.h b/LibSidServo/movingmodel.h index 142e138..d7b75bb 100644 --- a/LibSidServo/movingmodel.h +++ b/LibSidServo/movingmodel.h @@ -48,7 +48,7 @@ typedef struct{ } limits_t; typedef enum{ - STAGE_ACCEL, // start from zero speed and accelerate to Max speed + STAGE_ACCEL, // start from last speed and accelerate/decelerate to target speed STAGE_MAXSPEED, // go with target speed STAGE_DECEL, // go from target speed to zero STAGE_STOPPED, // stop @@ -68,7 +68,7 @@ typedef struct movemodel{ movestate_t (*get_state)(struct movemodel *m, moveparam_t *cur); // get current moving state void (*stop)(struct movemodel *m, double t); // stop by ramp void (*emergency_stop)(struct movemodel *m, double t); // stop with highest acceleration - double (*stoppedtime)(struct movemodel *m); // time when moving will ends + double (*stoppedtime)(struct movemodel *m); // time when moving will ends pthread_mutex_t mutex; } movemodel_t; diff --git a/LibSidServo/ramp.c b/LibSidServo/ramp.c index c0efe5e..6e88293 100644 --- a/LibSidServo/ramp.c +++ b/LibSidServo/ramp.c @@ -64,183 +64,136 @@ ret: pthread_mutex_unlock(&m->mutex); } +// inner part of `calc`, could be called recoursively for hard case +static void unlockedcalc(movemodel_t *m, moveparam_t *x, double t){ + // signs + double sign_a01 = 0., sign_a23 = 0., sign_vset = 0.; // accelerations on stages ACCEL and DECEL, speed on maxspeed stage + // times + double dt01 = 0., dt12 = 0., dt23 = 0.; + // absolute speed at stage 23 (or in that point); absolute max acceleration + double abs_vset = x->speed, abs_a = m->Max.accel; + // absolute target movement + double abs_Dx = fabs(x->coord - m->curparams.coord); + if(m->state == ST_STOP && abs_Dx < coord_tolerance){ + DBG("Movement too small -> stay at place"); + return; + } + // signs of Dx and current speed + double sign_Dx = (x->coord > m->curparams.coord) ? 1. : -1.; + double v0 = m->curparams.speed; + double sign_v0 = v0 < 0. ? -1 : 1., abs_v0 = fabs(v0); + if(v0 == 0.) sign_v0 = 0.; + // preliminary calculations (vset and dependent values could be changed) + dt01 = fabs(abs_v0 - abs_vset) / abs_a; + double abs_dx23 = abs_vset * abs_vset / 2. / abs_a; + dt23 = abs_vset / abs_a; + double abs_dx_stop = abs_v0 * abs_v0 / 2. / abs_a; + if(sign_Dx * sign_v0 >= 0. && abs_dx_stop < abs_Dx){ // we shouldn't change speed direction + if(fabs(abs_dx_stop - abs_Dx) <= coord_tolerance){ // simplest case: just stop + //DBG("Simplest case: stop"); + dt01 = dt12 = 0.; + sign_a23 = -sign_v0; + dt23 = abs_v0 / abs_a; + }else if(abs_vset < abs_v0){ // move with smaller speed than now: very simple case + //DBG("Move with smaller speed"); + sign_a01 = sign_a23 = -sign_v0; + sign_vset = sign_v0; + double abs_dx01 = abs_v0 * dt01 - abs_a * dt01 * dt01 / 2.; + double abs_dx12 = abs_Dx - abs_dx01 - abs_dx23; + dt12 = abs_dx12 / abs_vset; + }else{// move with larget speed + //DBG("Move with larger speed"); + double abs_dx01 = abs_v0 * dt01 + abs_a * dt01 * dt01 / 2.; + if(abs_Dx < abs_dx01 + abs_dx23){ // recalculate target speed and other + abs_vset = sqrt(abs_a * abs_Dx + abs_v0 * abs_v0 / 2.); + dt01 = fabs(abs_v0 - abs_vset) / abs_a; + abs_dx01 = abs_v0 * dt01 + abs_a * dt01 * dt01 / 2.; + dt23 = abs_vset / abs_a; + abs_dx23 = abs_vset * abs_vset / 2. / abs_a; + DBG("Can't reach target speed %g, take %g instead", x->speed, abs_vset); + } + sign_a01 = sign_Dx; // sign_v0 could be ZERO!!! + sign_a23 = -sign_Dx; + sign_vset = sign_Dx; + double abs_dx12 = abs_Dx - abs_dx01 - abs_dx23; + dt12 = abs_dx12 / abs_vset; + } + }else{ + // if we are here, we have the worst case: change speed direction + DBG("Hardest case: change speed direction"); + // now we should calculate coordinate at which model stops and biuld new trapezium from that point + double x0 = m->curparams.coord, v0 = m->curparams.speed; + double xstop = x0 + sign_v0 * abs_dx_stop, tstop = t + abs_v0 / abs_a; + m->state = ST_STOP; + m->curparams.accel = 0.; m->curparams.coord = xstop; m->curparams.speed = 0.; + unlockedcalc(m, x, tstop); // calculate new ramp + // and change started conditions + m->curparams.coord = x0; m->curparams.speed = v0; + m->Times[STAGE_ACCEL] = t; + m->Params[STAGE_ACCEL].coord = x0; + m->Params[STAGE_ACCEL].speed = v0; + DBG("NOW t[0]=%g, X[0]=%g, V[0]=%g", t, x0, v0); + return; + } + m->state = ST_MOVE; + m->movingstage = STAGE_ACCEL; + // some knot parameters + double a01 = sign_a01 * abs_a, a23 = sign_a23 * abs_a; + double v1, v2, x0, x1, x2; + v2 = v1 = sign_vset * abs_vset; + x0 = m->curparams.coord; + x1 = x0 + v0 * dt01 + a01 * dt01 * dt01 / 2.; + x2 = x1 + v1 * dt12; + // fill knot parameters + moveparam_t *p = &m->Params[STAGE_ACCEL]; // 0-1 - change started speed + p->accel = a01; + p->speed = m->curparams.speed; + p->coord = x0; + m->Times[STAGE_ACCEL] = t; + p = &m->Params[STAGE_MAXSPEED]; // 1-2 - constant speed + p->accel = 0.; + p->speed = v1; + p->coord = x1; + m->Times[STAGE_MAXSPEED] = m->Times[STAGE_ACCEL] + dt01; + p = &m->Params[STAGE_DECEL]; // 2-3 - decrease speed + p->accel = a23; + p->speed = v2; + p->coord = x2; + m->Times[STAGE_DECEL] = m->Times[STAGE_MAXSPEED] + dt12; + p = &m->Params[STAGE_STOPPED]; // 3 - stop at target + p->accel = p->speed = 0.; + p->coord = x->coord; + m->Times[STAGE_STOPPED] = m->Times[STAGE_DECEL] + dt23; +} + /** * @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(movemodel_t *m, moveparam_t *x, double t){ - if(!x) return FALSE; +static int calc(movemodel_t *m, moveparam_t *x, double t) { + //DBG("target coord/speed: %g/%g; current: %g/%g", x->coord, x->speed, m->curparams.coord, m->curparams.speed); + if (!x || !m) return FALSE; pthread_mutex_lock(&m->mutex); - DBG("target coord/speed: %g/%g; current coord: %g", x->coord, x->speed, m->curparams.coord); int ret = FALSE; + // Validate input parameters if(x->coord < m->Min.coord || x->coord > m->Max.coord){ - DBG("Wrong coordinage [%g, %g]", m->Min.coord, m->Max.coord); + DBG("Wrong coordinate [%g, %g]", m->Min.coord, m->Max.coord); goto ret; } if(x->speed < m->Min.speed || x->speed > m->Max.speed){ DBG("Wrong speed [%g, %g]", m->Min.speed, m->Max.speed); goto ret; } - double Dx = fabs(x->coord - m->curparams.coord); // full distance - double sign = (x->coord > m->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 / m->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 - m->Times[0] = t; - m->Params[0].speed = m->curparams.speed; - m->Params[0].coord = m->curparams.coord; - - double curspeed = fabs(m->curparams.speed); - 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); - 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); - stop(m, t); - ret = TRUE; - goto ret; - } - -#if 0 -model_move2 (/home/eddy/C-files/LibSidServo/movingmodel.c, line 63): MOVE to -0.785398 at speed 0.00408621 -calc (/home/eddy/C-files/LibSidServo/ramp.c, line 76): target coord/speed: -0.785398/0.00408621; current coord: 0.181616 -calc (/home/eddy/C-files/LibSidServo/ramp.c, line 91): Dx=0.967014, sign=-1, dt23=0.0185812, dx23=3.79634e-05 -calc (/home/eddy/C-files/LibSidServo/ramp.c, line 101): dt0s=0.0377057, dx0s=0.000156326, curspeed=0.0082919 -calc (/home/eddy/C-files/LibSidServo/ramp.c, line 110): SIGN of speed should be changed! -calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 0: t=3.9028, coord=0.181616, speed=0.0082919, accel=-0.219911 -calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 1: t=3.9405, coord=0.181772, speed=0, accel=-0.219911 -calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 2: t=3.95908, coord=0.181734, speed=-0.00408621, accel=0.219911 -calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 3: t=3.97766, coord=-0.785398, speed=0, accel=0 -model_move2 (/home/eddy/C-files/LibSidServo/movingmodel.c, line 63): MOVE to -0.785398 at speed 0.0361538 -calc (/home/eddy/C-files/LibSidServo/ramp.c, line 76): target coord/speed: -0.785398/0.0361538; current coord: 0.177343 -calc (/home/eddy/C-files/LibSidServo/ramp.c, line 91): Dx=0.962741, sign=-1, dt23=0.218049, dx23=0.00394164 -calc (/home/eddy/C-files/LibSidServo/ramp.c, line 101): dt0s=0.185651, dx0s=0.00285737, curspeed=0.0307821 -calc (/home/eddy/C-files/LibSidServo/ramp.c, line 110): SIGN of speed should be changed! -calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 0: t=3.9028, coord=0.177343, speed=0.0307821, accel=-0.165806 -calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 1: t=4.08845, coord=0.1802, speed=0, accel=-0.165806 -calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 2: t=4.3065, coord=0.176259, speed=-0.0361538, accel=0.165806 -calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 3: t=4.52455, coord=-0.785398, speed=0, accel=0 -proc (/home/eddy/C-files/LibSidServo/ramp.c, line 233): REACHED STOPping stage @ t=3.97837 -proc (/home/eddy/C-files/LibSidServo/ramp.c, line 235): T[3]=3.97766, -proc (/home/eddy/C-files/LibSidServo/ramp.c, line 235): T[2]=3.95908, -proc (/home/eddy/C-files/LibSidServo/ramp.c, line 235): T[1]=3.9405, -proc (/home/eddy/C-files/LibSidServo/ramp.c, line 235): T[0]=3.9028, - -#endif - 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; - if(sign * m->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 = sign*(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(m->Max.accel * Dx - curspeed * curspeed / 2.); - if(setspeed < curspeed){ - setspeed = curspeed; - dt01 = 0.; dx01 = 0.; - m->Params[0].accel = 0.; - }else{ - m->Params[0].accel = a; - dt01 = fabs(setspeed - curspeed) / m->Max.accel; - dx01 = curspeed * dt01 + m->Max.accel * dt01 * dt01 / 2.; - } - }else m->Params[0].accel = a; - } - if(setspeed < m->Min.speed){ - DBG("New speed (%g) should be too small (<%g)", setspeed, m->Min.speed); - goto ret; - } - moveparam_t *p = &m->Params[STAGE_MAXSPEED]; - p->accel = 0.; p->speed = sign * setspeed; - p->coord = m->curparams.coord + dx01 * sign; - m->Times[STAGE_MAXSPEED] = m->Times[0] + dt01; - dt23 = setspeed / m->Max.accel; - dx23 = setspeed * dt23 / 2.; - DBG("setspeed=%g, dt23=%g, tx23=%g", setspeed, dt23, dx23); - // calculate dx12 and dt12 - double dx12 = Dx - dx01 - dx23; - if(dx12 < -coord_tolerance){ - DBG("Oops, WTF dx12=%g?", dx12); - goto ret; - } - double dt12 = dx12 / setspeed; - p = &m->Params[STAGE_DECEL]; - p->accel = -sign * m->Max.accel; - p->speed = sign * setspeed; - p->coord = m->Params[STAGE_MAXSPEED].coord + sign * dx12; - m->Times[STAGE_DECEL] = m->Times[STAGE_MAXSPEED] + dt12; - 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 = TRUE; // now there's no chanses to make error + unlockedcalc(m, x, t); + // Debug output + /*for(int i = 0; i < STAGE_AMOUNT; i++){ + DBG("Stage %d: t=%.6f, coord=%.6f, speed=%.6f, accel=%.6f", + i, m->Times[i], m->Params[i].coord, m->Params[i].speed, m->Params[i].accel); + }*/ 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; }