eddy/mountcontrol
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

start changing T-ramp code (old was too ugly)

Browse Source
This commit is contained in:
eddyem
2025-04-13 17:59:51 +03:00
parent d5dd260007
commit 76ddd60c7d
5 changed files with 114 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
moving_model/Tramp.c
View File

@@ -166,7 +166,7 @@ static int calcfromgs(moveparam_t *x, double t){
// calculations from non-stopped state
static int calcfrommove(moveparam_t *x, double t){
double sign = (x->coord > curparams.coord) ? 1. : -1.; // signum of target accelerations and speeds
double curspdsign = (curparams.speed > 0.) ? 1. : -1.;
double curspeedsign = (curparams.speed > 0.) ? 1. : -1.;
double absspeed = curparams.speed * sign; // abs speed value
double dt = absspeed / Max.accel; // time to accelerate to current speed
// check if target isn't too close for move in stopped mode
@@ -179,7 +179,7 @@ static int calcfrommove(moveparam_t *x, double t){
return FALSE; // can't immediatelly stop
}
if(x->speed < absspeed) return calcfromgs(x, t);
if(sign * curspdsign > 0.){ // move into same side we are moving
if(sign * curspeedsign > 0.){ // move into same side we are moving
return calcfromstop(x, t-dt, curparams.coord - xacc*sign); // just think that we are moving from past
}else{ // move into opposite side: here we can't use trick with "moving from past"
return calcfromopp(x, t);
@@ -196,8 +196,68 @@ 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;
if(state == ST_STOP) return calcfromstop(x, t, curparams.coord);
else return calcfrommove(x, t);
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;
dx01 = dx0s + setspeed / Max.accel / 2.;
}else{ // increase or decrease speed without stopping phase
dt01 = fabs(sign*setspeed - curparams.speed);
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(setspeed < Min.speed){
DBG("New speed should be too small");
return FALSE;
}
moveparam_t *p = &Params[STAGE_MAXSPEED];
p->accel = 0.; p->speed = setspeed;
p->coord = curparams.coord + dx01 * sign;
dt23 = setspeed / Max.accel;
dx23 = setspeed * dt23 / 2.;
// calculate dx12 and dt12
double dx12 = Dx - dx01 - dx23;
if(dx12 < 0.){
DBG("Oops, WTF?");
return FALSE;
}
double dt12 = dx12 / setspeed;
p = &Params[STAGE_DECEL];
p->accel = -sign * Max.accel;
p->speed = setspeed;
p->coord = Params[STAGE_MAXSPEED].coord + sign * dx12;
;;
}
static movestate_t proc(moveparam_t *next, double t){