Almost done trapezioidal model

This commit is contained in:
eddyem 2025-04-11 22:14:43 +03:00
parent 33d7800a62
commit 89d67fd95c
5 changed files with 246 additions and 27 deletions

View File

@ -78,7 +78,7 @@ static int calc(moveparam_t *x, double t){
T1 = t + ticks_need * time_tick;
}
}
state = ST_CONSTSPEED;
state = ST_MOVE;
Xlast0 = curparams.coord;
if(target.speed > Min.speed) movingstage = STAGE_NORMALSPEED;
else movingstage = STAGE_MINSPEED;
@ -87,7 +87,7 @@ static int calc(moveparam_t *x, double t){
return TRUE;
}
static void stop(){
static void stop(double _U_ t){
T0 = -1.;
curparams.accel = 0.;
curparams.speed = 0.;
@ -104,7 +104,7 @@ static movestate_t proc(moveparam_t *next, double t){
else{ if(curparams.coord > target.coord) ooops = TRUE; }
if(ooops){
DBG("OOOps! We are (%g) over target (%g) -> stop", curparams.coord, target.coord);
stop();
stop(t);
if(next) *next = curparams;
return state;
}
@ -120,7 +120,7 @@ static movestate_t proc(moveparam_t *next, double t){
}
if(fabs(curparams.coord - target.coord) < coord_tolerance){ // we are at place
DBG("OK, we are in place");
stop();
stop(t);
}
if(next) *next = curparams;
return state;

View File

@ -18,12 +18,229 @@
// 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
movemodel_t trapez = { 0 };
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 curspdsign = (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 * curspdsign > 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;
if(state == ST_STOP) return calcfromstop(x, t, curparams.coord);
else return calcfrommove(x, t);
}
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,
};

View File

@ -16,6 +16,7 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <math.h>
#include <stdio.h>
#include <usefull_macros.h>
#include <pthread.h>
@ -57,7 +58,7 @@ static void* thread(void _U_ *u){
//pthread_mutex_lock(&mutex);
movestate_t curstate = model->get_state(NULL);
moveparam_t curmove;
if(curstate != ST_STOP && curstate != ST_ERROR){
if(curstate == ST_MOVE){
curstate = model->proc_move(&curmove, t);
}
//pthread_mutex_unlock(&mutex);

View File

@ -35,14 +35,11 @@ typedef enum{
typedef enum{
ST_STOP, // stopped
ST_ACC, // accelerated
ST_CONSTSPEED, // moving with constant speed
ST_DEC, // decelerated
ST_ERROR, // some error -> stop
ST_MOVE, // moving
ST_AMOUNT
} movestate_t;
typedef struct{
typedef struct{ // all values could be both as positive and negative
double coord;
double speed;
double accel;
@ -59,10 +56,13 @@ typedef struct{
int (*calculate)(moveparam_t *target, double t); // calculate stages of traectory beginning from t
movestate_t (*proc_move)(moveparam_t *next, double t); // calculate next model point for time t
movestate_t (*get_state)(moveparam_t *cur); // get current moving state
void (*stop)(); // stop by ramp
void (*emergency_stop); // stop with highest acceleration
void (*stop)(double t); // stop by ramp
void (*emergency_stop)(double t); // stop with highest acceleration
double (*stoppedtime)(); // time when moving will ends
} movemodel_t;
extern double coord_tolerance;
double nanot();
movemodel_t *init_moving(ramptype_t type, limits_t *l);
int init_coordtol(double tolerance);

View File

@ -1,10 +1,10 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 16.0.0, 2025-03-18T22:08:04. -->
<!-- Written by QtCreator 15.0.1, 2025-04-11T22:14:27. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
<value type="QByteArray">{7bd84e39-ca37-46d3-be9d-99ebea85bc0d}</value>
<value type="QByteArray">{cf63021e-ef53-49b0-b03b-2f2570cdf3b6}</value>
</data>
<data>
<variable>ProjectExplorer.Project.ActiveTarget</variable>
@ -13,8 +13,8 @@
<data>
<variable>ProjectExplorer.Project.EditorSettings</variable>
<valuemap type="QVariantMap">
<value type="bool" key="EditorConfiguration.AutoDetect">true</value>
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
<value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
<value type="QString" key="language">Cpp</value>
@ -42,7 +42,7 @@
<value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">1</value>
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
@ -79,7 +79,7 @@
<value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
<value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
<value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
<value type="int" key="ClangTools.ParallelJobs">8</value>
<value type="int" key="ClangTools.ParallelJobs">1</value>
<value type="bool" key="ClangTools.PreferConfigFile">true</value>
<valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
<valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
@ -94,12 +94,12 @@
<value type="QString" key="DeviceType">Desktop</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{65a14f9e-e008-4c1b-89df-4eaa4774b6e3}</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{91347f2c-5221-46a7-80b1-0a054ca02f79}</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/Big/Data/00__Small_tel/moving_model</value>
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/eddy/C-files/mountcontrol.git/moving_model</value>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
@ -109,8 +109,8 @@
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Build</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Сборка</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Сборка</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
</valuemap>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
@ -122,8 +122,8 @@
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Clean</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Очистка</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Очистка</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
@ -131,15 +131,15 @@
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.CustomParsers"/>
<value type="bool" key="ProjectExplorer.BuildConfiguration.ParseStandardOutput">false</value>
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Default</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">По умолчанию</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
@ -153,6 +153,7 @@
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
<value type="bool" key="Analyzer.Valgrind.ShowReachable">true</value>
<value type="QList&lt;int&gt;" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
<valuelist type="QVariantList" key="CustomOutputParsers"/>
<value type="int" key="PE.EnvironmentAspect.Base">2</value>