307 lines
12 KiB
C
307 lines
12 KiB
C
/*
|
|
* This file is part of the moving_model project.
|
|
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
|
*
|
|
* This program is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
// simplest trapezioidal ramp
|
|
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
#include <strings.h>
|
|
#include <usefull_macros.h>
|
|
|
|
#include "Tramp.h"
|
|
|
|
static movestate_t state = ST_STOP;
|
|
static moveparam_t target, Min, Max; // `Min` acceleration not used!
|
|
|
|
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
|
|
|
|
int initlims(limits_t *lim){
|
|
if(!lim) return FALSE;
|
|
Min = lim->min;
|
|
Max = lim->max;
|
|
return TRUE;
|
|
}
|
|
|
|
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;
|
|
// 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;
|
|
// 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.;
|
|
}
|
|
|
|
// 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
|
|
* @param t - current time value
|
|
* @return FALSE if can't move with given parameters
|
|
*/
|
|
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;
|
|
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){
|
|
if(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;
|
|
break;
|
|
}
|
|
}
|
|
if(movingstage == STAGE_STOPPED){
|
|
curparams.coord = Params[STAGE_STOPPED].coord;
|
|
emstop(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.;
|
|
ret:
|
|
if(next) *next = curparams;
|
|
return state;
|
|
}
|
|
|
|
static movestate_t getst(moveparam_t *cur){
|
|
if(cur) *cur = curparams;
|
|
return state;
|
|
}
|
|
|
|
static double gettstop(){
|
|
return Times[STAGE_STOPPED];
|
|
}
|
|
|
|
movemodel_t trapez = {
|
|
.init_limits = initlims,
|
|
.stop = stop,
|
|
.emergency_stop = emstop,
|
|
.get_state = getst,
|
|
.calculate = calc,
|
|
.proc_move = proc,
|
|
.stoppedtime = gettstop,
|
|
};
|