less squares 4 speed + fixed some bugs (but found more)
This commit is contained in:
@@ -25,53 +25,38 @@
|
||||
#include "ramp.h"
|
||||
|
||||
static double coord_tolerance = COORD_TOLERANCE_DEFAULT;
|
||||
static movestate_t state = ST_STOP;
|
||||
static moveparam_t Min, Max;
|
||||
|
||||
typedef enum{
|
||||
STAGE_ACCEL, // start from zero speed and accelerate to Max speed
|
||||
STAGE_MAXSPEED, // go with target speed
|
||||
STAGE_DECEL, // go from target speed to zero
|
||||
STAGE_STOPPED, // stop
|
||||
STAGE_AMOUNT
|
||||
} 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 moveparam_t curparams = {0}; // current coordinate/speed/acceleration
|
||||
|
||||
static int initlims(limits_t *lim){
|
||||
if(!lim) return FALSE;
|
||||
Min = lim->min;
|
||||
Max = lim->max;
|
||||
return TRUE;
|
||||
static void emstop(movemodel_t *m, double _U_ t){
|
||||
FNAME();
|
||||
pthread_mutex_lock(&m->mutex);
|
||||
m->curparams.accel = 0.;
|
||||
m->curparams.speed = 0.;
|
||||
bzero(m->Times, sizeof(double) * STAGE_AMOUNT);
|
||||
bzero(m->Params, sizeof(moveparam_t) * STAGE_AMOUNT);
|
||||
m->state = ST_STOP;
|
||||
m->movingstage = STAGE_STOPPED;
|
||||
pthread_mutex_unlock(&m->mutex);
|
||||
}
|
||||
|
||||
static void emstop(double _U_ t){
|
||||
curparams.accel = 0.;
|
||||
curparams.speed = 0.;
|
||||
bzero(Times, sizeof(Times));
|
||||
bzero(Params, sizeof(Params));
|
||||
state = ST_STOP;
|
||||
movingstage = STAGE_STOPPED;
|
||||
}
|
||||
|
||||
static void stop(double t){
|
||||
if(state == ST_STOP || movingstage == STAGE_STOPPED) return;
|
||||
movingstage = STAGE_DECEL;
|
||||
state = ST_MOVE;
|
||||
Times[STAGE_DECEL] = t;
|
||||
Params[STAGE_DECEL].speed = curparams.speed;
|
||||
if(curparams.speed > 0.) Params[STAGE_DECEL].accel = -Max.accel;
|
||||
else Params[STAGE_DECEL].accel = Max.accel;
|
||||
Params[STAGE_DECEL].coord = curparams.coord;
|
||||
static void stop(movemodel_t *m, double t){
|
||||
FNAME();
|
||||
pthread_mutex_lock(&m->mutex);
|
||||
if(m->state == ST_STOP || m->movingstage == STAGE_STOPPED) goto ret;
|
||||
m->movingstage = STAGE_DECEL;
|
||||
m->state = ST_MOVE;
|
||||
m->Times[STAGE_DECEL] = t;
|
||||
m->Params[STAGE_DECEL].speed = m->curparams.speed;
|
||||
if(m->curparams.speed > 0.) m->Params[STAGE_DECEL].accel = -m->Max.accel;
|
||||
else m->Params[STAGE_DECEL].accel = m->Max.accel;
|
||||
m->Params[STAGE_DECEL].coord = m->curparams.coord;
|
||||
// speed: v=v2+a2(t-t2), v2 and a2 have different signs; t3: v3=0 -> t3=t2-v2/a2
|
||||
Times[STAGE_STOPPED] = t - curparams.speed / Params[STAGE_DECEL].accel;
|
||||
m->Times[STAGE_STOPPED] = t - m->curparams.speed / m->Params[STAGE_DECEL].accel;
|
||||
// coordinate: x=x2+v2(t-t2)+a2(t-t2)^2/2 -> x3=x2+v2(t3-t2)+a2(t3-t2)^2/2
|
||||
double dt = Times[STAGE_STOPPED] - t;
|
||||
Params[STAGE_STOPPED].coord = curparams.coord + curparams.speed * dt +
|
||||
Params[STAGE_DECEL].accel * dt * dt / 2.;
|
||||
double dt = m->Times[STAGE_STOPPED] - t;
|
||||
m->Params[STAGE_STOPPED].coord = m->curparams.coord + m->curparams.speed * dt +
|
||||
m->Params[STAGE_DECEL].accel * dt * dt / 2.;
|
||||
ret:
|
||||
pthread_mutex_unlock(&m->mutex);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -80,141 +65,166 @@ static void stop(double t){
|
||||
* @param t - current time value
|
||||
* @return FALSE if can't move with given parameters
|
||||
*/
|
||||
static int calc(moveparam_t *x, double t){
|
||||
static int calc(movemodel_t *m, moveparam_t *x, double t){
|
||||
DBG("coord/speed: %g/%g", x->coord, x->speed);
|
||||
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;
|
||||
double Dx = fabs(x->coord - curparams.coord); // full distance
|
||||
double sign = (x->coord > curparams.coord) ? 1. : -1.; // sign of target accelerations and speeds
|
||||
pthread_mutex_lock(&m->mutex);
|
||||
int ret = FALSE;
|
||||
if(x->coord < m->Min.coord || x->coord > m->Max.coord){
|
||||
DBG("Wrong coordinage [%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 / Max.accel; // time of deceleration stage for given 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
|
||||
Times[0] = t;
|
||||
Params[0].speed = curparams.speed;
|
||||
Params[0].coord = curparams.coord;
|
||||
m->Times[0] = t;
|
||||
m->Params[0].speed = m->curparams.speed;
|
||||
m->Params[0].coord = m->curparams.coord;
|
||||
|
||||
double curspeed = fabs(curparams.speed);
|
||||
double dt0s = curspeed / Max.accel; // time of stopping phase
|
||||
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", dt0s, dx0s);
|
||||
if(dx0s > Dx){
|
||||
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");
|
||||
return FALSE;
|
||||
goto ret;
|
||||
}
|
||||
if(fabs(Dx - dx0s) < coord_tolerance){ // just stop and we'll be on target
|
||||
DBG("Distance good to just stop");
|
||||
stop(t);
|
||||
return TRUE;
|
||||
pthread_mutex_unlock(&m->mutex);
|
||||
stop(m, t);
|
||||
ret = TRUE;
|
||||
goto ret;
|
||||
}
|
||||
if(curparams.speed * sign < 0. || state == ST_STOP){ // we should change speed sign
|
||||
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(Max.accel * dxs3);
|
||||
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 - curparams.speed) / Max.accel;
|
||||
Params[0].accel = sign * Max.accel;
|
||||
if(state == ST_STOP) dx01 = setspeed * dt01 / 2.;
|
||||
else dx01 = dt01 * (dt01 / 2. * Max.accel - curspeed);
|
||||
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);
|
||||
}else{ // increase or decrease speed without stopping phase
|
||||
dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel;
|
||||
double a = sign * Max.accel;
|
||||
if(sign * curparams.speed < 0.){DBG("change direction"); a = -a;}
|
||||
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 = curspeed * dt01 + a * dt01 * dt01 / 2.;
|
||||
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(Max.accel * Dx - curspeed * curspeed / 2.);
|
||||
setspeed = sqrt(m->Max.accel * Dx - curspeed * curspeed / 2.);
|
||||
if(setspeed < curspeed){
|
||||
setspeed = curparams.speed;
|
||||
setspeed = m->curparams.speed;
|
||||
dt01 = 0.; dx01 = 0.;
|
||||
Params[0].accel = 0.;
|
||||
m->Params[0].accel = 0.;
|
||||
}else{
|
||||
Params[0].accel = a;
|
||||
dt01 = fabs(setspeed - curspeed) / Max.accel;
|
||||
dx01 = curspeed * dt01 + Max.accel * dt01 * dt01 / 2.;
|
||||
m->Params[0].accel = a;
|
||||
dt01 = fabs(setspeed - curspeed) / m->Max.accel;
|
||||
dx01 = curspeed * dt01 + m->Max.accel * dt01 * dt01 / 2.;
|
||||
}
|
||||
}else Params[0].accel = a;
|
||||
}else m->Params[0].accel = a;
|
||||
}
|
||||
if(setspeed < Min.speed){
|
||||
DBG("New speed should be too small");
|
||||
return FALSE;
|
||||
if(setspeed < m->Min.speed){
|
||||
DBG("New speed (%g) should be too small (<%g)", setspeed, m->Min.speed);
|
||||
goto ret;
|
||||
}
|
||||
moveparam_t *p = &Params[STAGE_MAXSPEED];
|
||||
moveparam_t *p = &m->Params[STAGE_MAXSPEED];
|
||||
p->accel = 0.; p->speed = sign * setspeed;
|
||||
p->coord = curparams.coord + dx01 * sign;
|
||||
Times[STAGE_MAXSPEED] = Times[0] + dt01;
|
||||
dt23 = setspeed / Max.accel;
|
||||
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);
|
||||
return FALSE;
|
||||
goto ret;
|
||||
}
|
||||
double dt12 = dx12 / setspeed;
|
||||
p = &Params[STAGE_DECEL];
|
||||
p->accel = -sign * Max.accel;
|
||||
p = &m->Params[STAGE_DECEL];
|
||||
p->accel = -sign * m->Max.accel;
|
||||
p->speed = sign * setspeed;
|
||||
p->coord = Params[STAGE_MAXSPEED].coord + sign * dx12;
|
||||
Times[STAGE_DECEL] = Times[STAGE_MAXSPEED] + dt12;
|
||||
p = &Params[STAGE_STOPPED];
|
||||
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;
|
||||
Times[STAGE_STOPPED] = Times[STAGE_DECEL] + dt23;
|
||||
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,
|
||||
Times[i], Params[i].coord, Params[i].speed, Params[i].accel);
|
||||
state = ST_MOVE;
|
||||
movingstage = STAGE_ACCEL;
|
||||
return TRUE;
|
||||
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:
|
||||
pthread_mutex_unlock(&m->mutex);
|
||||
return ret;
|
||||
}
|
||||
|
||||
static movestate_t proc(moveparam_t *next, double t){
|
||||
if(state == ST_STOP) goto ret;
|
||||
static movestate_t proc(movemodel_t *m, moveparam_t *next, double t){
|
||||
pthread_mutex_lock(&m->mutex);
|
||||
if(m->state == ST_STOP) goto ret;
|
||||
for(movingstage_t s = STAGE_STOPPED; s >= 0; --s){
|
||||
if(Times[s] <= t){ // check time for current stage
|
||||
movingstage = s;
|
||||
if(m->Times[s] <= t){ // check time for current stage
|
||||
m->movingstage = s;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if(movingstage == STAGE_STOPPED){
|
||||
curparams.coord = Params[STAGE_STOPPED].coord;
|
||||
emstop(t);
|
||||
if(m->movingstage == STAGE_STOPPED){
|
||||
m->curparams.coord = m->Params[STAGE_STOPPED].coord;
|
||||
pthread_mutex_unlock(&m->mutex);
|
||||
emstop(m, t);
|
||||
goto ret;
|
||||
}
|
||||
// calculate current parameters
|
||||
double dt = t - Times[movingstage];
|
||||
double a = Params[movingstage].accel;
|
||||
double v0 = Params[movingstage].speed;
|
||||
double x0 = Params[movingstage].coord;
|
||||
curparams.accel = a;
|
||||
curparams.speed = v0 + a * dt;
|
||||
curparams.coord = x0 + v0 * dt + a * dt * dt / 2.;
|
||||
double dt = t - m->Times[m->movingstage];
|
||||
double a = m->Params[m->movingstage].accel;
|
||||
double v0 = m->Params[m->movingstage].speed;
|
||||
double x0 = m->Params[m->movingstage].coord;
|
||||
m->curparams.accel = a;
|
||||
m->curparams.speed = v0 + a * dt;
|
||||
m->curparams.coord = x0 + v0 * dt + a * dt * dt / 2.;
|
||||
ret:
|
||||
if(next) *next = curparams;
|
||||
return state;
|
||||
if(next) *next = m->curparams;
|
||||
movestate_t st = m->state;
|
||||
pthread_mutex_unlock(&m->mutex);
|
||||
return st;
|
||||
}
|
||||
|
||||
static movestate_t getst(moveparam_t *cur){
|
||||
if(cur) *cur = curparams;
|
||||
return state;
|
||||
static movestate_t getst(movemodel_t *m, moveparam_t *cur){
|
||||
pthread_mutex_lock(&m->mutex);
|
||||
if(cur) *cur = m->curparams;
|
||||
movestate_t st = m->state;
|
||||
pthread_mutex_unlock(&m->mutex);
|
||||
return st;
|
||||
}
|
||||
|
||||
static double gettstop(){
|
||||
return Times[STAGE_STOPPED];
|
||||
static double gettstop(movemodel_t *m){
|
||||
pthread_mutex_lock(&m->mutex);
|
||||
double r = m->Times[STAGE_STOPPED];
|
||||
pthread_mutex_unlock(&m->mutex);
|
||||
return r;
|
||||
}
|
||||
|
||||
movemodel_t trapez = {
|
||||
.init_limits = initlims,
|
||||
.stop = stop,
|
||||
.emergency_stop = emstop,
|
||||
.get_state = getst,
|
||||
.calculate = calc,
|
||||
.proc_move = proc,
|
||||
.stoppenanotime = gettstop,
|
||||
.stoppedtime = gettstop,
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user