Indeed? The moving model works???
This commit is contained in:
parent
6315d5e18e
commit
2fee216924
@ -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;
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -1,7 +0,0 @@
|
||||
# configuration file for SSII driven equatorial mount
|
||||
verbose = 5
|
||||
mountpath = /dev/ttyS1
|
||||
mountspeed = 19200
|
||||
encoderpath = /dev/ttyUSB0
|
||||
encoderspeed = 153000
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user