306 lines
13 KiB
C
306 lines
13 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 <strings.h>
|
|
|
|
#include "main.h"
|
|
#include "ramp.h"
|
|
/*
|
|
#ifdef EBUG
|
|
#undef DBG
|
|
#define DBG(...)
|
|
#endif
|
|
*/
|
|
static double coord_tolerance = COORD_TOLERANCE_DEFAULT;
|
|
|
|
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 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
|
|
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 = 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);
|
|
}
|
|
|
|
/**
|
|
* @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;
|
|
pthread_mutex_lock(&m->mutex);
|
|
DBG("target coord/speed: %g/%g; current coord: %g", x->coord, x->speed, m->curparams.coord);
|
|
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 / 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:
|
|
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;
|
|
}
|
|
|
|
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(m->Times[s] <= t){ // check time for current stage
|
|
m->movingstage = s;
|
|
break;
|
|
}
|
|
}
|
|
if(m->movingstage == STAGE_STOPPED){
|
|
m->curparams.coord = m->Params[STAGE_STOPPED].coord;
|
|
pthread_mutex_unlock(&m->mutex);
|
|
DBG("REACHED STOPping stage @ t=%g", t);
|
|
for(int s = STAGE_STOPPED; s >= 0; --s){
|
|
DBG("T[%d]=%g, ", s, m->Times[s]);
|
|
}
|
|
fflush(stdout);
|
|
emstop(m, t);
|
|
goto ret;
|
|
}
|
|
// calculate current parameters
|
|
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 = m->curparams;
|
|
movestate_t st = m->state;
|
|
pthread_mutex_unlock(&m->mutex);
|
|
return st;
|
|
}
|
|
|
|
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(movemodel_t *m){
|
|
pthread_mutex_lock(&m->mutex);
|
|
double r = m->Times[STAGE_STOPPED];
|
|
pthread_mutex_unlock(&m->mutex);
|
|
return r;
|
|
}
|
|
|
|
movemodel_t trapez = {
|
|
.stop = stop,
|
|
.emergency_stop = emstop,
|
|
.get_state = getst,
|
|
.calculate = calc,
|
|
.proc_move = proc,
|
|
.stoppedtime = gettstop,
|
|
};
|