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

T-ramp tested, seems like it works fine

Browse Source
This commit is contained in:
eddyem
2025-04-14 21:39:06 +03:00
parent 76ddd60c7d
commit a496c5cc54
7 changed files with 249 additions and 133 deletions
Download Patch File Download Diff File Expand all files Collapse all files

155
moving_model/Tramp.c
View File

@@ -26,7 +26,7 @@
#include "Tramp.h"
static movestate_t state = ST_STOP;
static moveparam_t target, Min, Max; // `Min` acceleration not used!
static moveparam_t Min, Max; // `Min` acceleration not used!
typedef enum{
STAGE_ACCEL, // start from zero speed and accelerate to Max speed
@@ -37,11 +37,11 @@ typedef enum{
} movingstage_t;
static movingstage_t movingstage = STAGE_STOPPED;
static double Times[STAGE_AMOUNT] = {0.}; // time when each stage starts
static moveparam_t Params[STAGE_AMOUNT] = {0.}; // starting parameters for each stage
static double Times[STAGE_AMOUNT] = {0}; // time when each stage starts
static moveparam_t Params[STAGE_AMOUNT] = {0}; // starting parameters for each stage
static moveparam_t curparams = {0}; // current coordinate/speed/acceleration
int initlims(limits_t *lim){
static int initlims(limits_t *lim){
if(!lim) return FALSE;
Min = lim->min;
Max = lim->max;
@@ -74,118 +74,6 @@ static void stop(double t){
Params[STAGE_DECEL].accel * dt * dt / 2.;
}
// calculations from stopped state
static int calcfromstop(moveparam_t *x, double t, double xstart){
// coordinate shift
double Dx = fabs(x->coord - xstart); // 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 dtacc = x->speed / Max.accel; // time to reach given speed
double dxacc = x->speed * dtacc; // distance on acc/dec stages
// without constant speed stage we have: 01) x=x0+at^2/2, 12)absent, 23) x=x2+v2*t-at^2/2
// so for full stage DX should be greater than v^2/2a+v^2/2a=v^2/a (or v*dt)
Times[0] = t;
Params[0].accel = sign * Max.accel;
Params[0].coord = xstart;
Params[0].speed = 0.;
if(Dx > 2. * dxacc){ // full stage
// time and moving on accelerated/decelerated stage
moveparam_t *p = &Params[STAGE_MAXSPEED];
p->accel = 0.; p->speed = x->speed; p->coord = xstart + dxacc * sign;
Times[STAGE_MAXSPEED] = t + dtacc;
p = &Params[STAGE_DECEL];
p->accel = -sign * Max.accel; p->coord = x->coord - sign * dxacc; p->speed = x->speed;
Times[STAGE_DECEL] = Times[STAGE_MAXSPEED] + (Dx - 2. * dxacc) / x->speed;
p = &Params[STAGE_STOPPED];
p->speed = 0.; p->accel = 0.; p->coord = x->coord;
Times[STAGE_STOPPED] = Times[STAGE_DECEL] + dtacc;
}else{ // short stage
// calculate max speed
double maxspeed = sqrt(2. * Max.accel * Dx);
if(maxspeed < Min.speed) return FALSE; // can't reach
// full traveling time
double fullt = Dx / maxspeed;
Times[STAGE_MAXSPEED] = Times[STAGE_DECEL] = t + fullt / 2.;
Times[STAGE_STOPPED] = t + fullt;
moveparam_t *p = &Params[STAGE_MAXSPEED];
p->accel = 0.; p->speed = maxspeed * sign; p->coord = xstart + Dx / 2. * sign;
p = &Params[STAGE_DECEL];
p->accel = -sign * Max.accel;
p->coord = Params[STAGE_MAXSPEED].coord;
p->speed = Params[STAGE_MAXSPEED].speed;
p = &Params[STAGE_STOPPED];
p->speed = 0.; p->accel = 0.; p->coord = x->coord;
}
if(Times[STAGE_STOPPED] < t) return FALSE;
return TRUE;
}
// calculations for moving into opposite side
static int calcfromopp(moveparam_t *x, double 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 dtdec = x->speed / Max.accel; // time of deceleration stage
double dxacc = x->speed * dtdec; // distance on dec stage (abs)
Times[0] = t;
Params[0].accel = sign * Max.accel;
Params[0].coord = curparams.coord;
Params[0].speed = curparams.speed;
double dt01 = (sign * x->speed - curparams.speed) / Params[0].accel; // time to reach target speed
if(dt01 < 0){ DBG("WTF? Got dt01=%g", dt01); return FALSE; }
double dx01 = curparams.speed * dt01 + Params[0].accel / 2. * dt01 * dt01; // distance on accel stage (signed)
if(Dx > dxacc + fabs(dx01)){ // full stage
;
}else{ // short stage
double absspeed0 = fabs(curparams.speed); // current speed abs val
double timetozs = absspeed0 / Max.accel; // time to zero speed on acceleration stage
double disttozs = absspeed0 * timetozs / 2.; // distance till zero speed
if(disttozs > Dx){DBG("Need to stop more than have"); return FALSE;}
double dxrem = Dx - disttozs; // remaining
double maxspeed = sqrt(2. * Max.accel * dxrem);
if(maxspeed < Min.speed) return FALSE;
double fullremt = dxrem / maxspeed;
Times[STAGE_MAXSPEED] = Times[STAGE_DECEL] = t + timetozs + fullremt / 2.;
Times[STAGE_STOPPED] = Times[STAGE_DECEL] + fullremt / 2.;
moveparam_t *p = &Params[STAGE_MAXSPEED];
p->accel = 0.; p->speed = maxspeed * sign; p->coord = curparams.coord + (disttozs + dxrem / 2.) * sign;
p = &Params[STAGE_DECEL];
p->accel = -sign * Max.accel;
p->coord = Params[STAGE_MAXSPEED].coord;
p->speed = Params[STAGE_MAXSPEED].speed;
p = &Params[STAGE_STOPPED];
p->speed = 0.; p->accel = 0.; p->coord = x->coord;
}
}
// calculations for moving from greater speed
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 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
double xacc = Max.accel * dt * dt / 2.; // acc/dec part
double dx = absspeed * dt - xacc;
double Dx = fabs(x->coord - curparams.coord); // total position shift
if(dx > Dx) return FALSE; // can't reach target in normal moving mode
if(Dx < coord_tolerance){
if(state == ST_STOP) return TRUE;
return FALSE; // can't immediatelly stop
}
if(x->speed < absspeed) return calcfromgs(x, t);
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);
}
}
/**
* @brief calc - moving calculation
* @param x - using max speed (>0!!!) and coordinate
@@ -229,21 +117,35 @@ static int calc(moveparam_t *x, double t){
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.;
if(state == ST_STOP) dx01 = setspeed * dt01 / 2.;
else dx01 = dt01 * (dt01 / 2. * Max.accel - curspeed);
DBG("dx01=%g, dt01=%g", dx01, dt01);
}else{ // increase or decrease speed without stopping phase
dt01 = fabs(sign*setspeed - curparams.speed);
dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel;
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(dx01 + dx23 > Dx){ // calculate max speed
setspeed = sqrt(Max.accel * Dx - curspeed * curspeed / 2.);
if(setspeed < curspeed){
setspeed = curparams.speed;
dt01 = 0.; dx01 = 0.;
Params[0].accel = 0.;
}else{
Params[0].accel = a;
dt01 = fabs(setspeed - curspeed) / Max.accel;
dx01 = curspeed * dt01 + Max.accel * dt01 * dt01 / 2.;
}
}else Params[0].accel = a;
}
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->accel = 0.; p->speed = sign * setspeed;
p->coord = curparams.coord + dx01 * sign;
Times[STAGE_MAXSPEED] = Times[0] + dt01;
dt23 = setspeed / Max.accel;
dx23 = setspeed * dt23 / 2.;
// calculate dx12 and dt12
@@ -255,9 +157,18 @@ static int calc(moveparam_t *x, double t){
double dt12 = dx12 / setspeed;
p = &Params[STAGE_DECEL];
p->accel = -sign * Max.accel;
p->speed = setspeed;
p->speed = sign * setspeed;
p->coord = Params[STAGE_MAXSPEED].coord + sign * dx12;
;;
Times[STAGE_DECEL] = Times[STAGE_MAXSPEED] + dt12;
p = &Params[STAGE_STOPPED];
p->accel = 0.; p->speed = 0.; p->coord = x->coord;
Times[STAGE_STOPPED] = Times[STAGE_DECEL] + dt23;
for(int i = 0; i < 4; ++i)
DBG("%d: t=%g, coord=%g, speed=%g, accel=%g", i,
Times[i], Params[i].coord, Params[i].speed, Params[i].accel);
state = ST_MOVE;
movingstage = STAGE_ACCEL;
return TRUE;
}
static movestate_t proc(moveparam_t *next, double t){