From a496c5cc54a7c5cc32d12821915bc10fa6975003 Mon Sep 17 00:00:00 2001 From: eddyem Date: Mon, 14 Apr 2025 21:39:06 +0300 Subject: [PATCH] T-ramp tested, seems like it works fine --- moving_model/Dramp.c | 2 +- moving_model/Tramp.c | 155 ++++----------- moving_model/main.c | 28 ++- moving_model/moving_model.creator.user | 2 +- .../moving_model.creator.user.7bd84e3 | 184 ++++++++++++++++++ moving_model/plot | 6 + moving_model/plot_pdf | 5 + 7 files changed, 249 insertions(+), 133 deletions(-) create mode 100644 moving_model/moving_model.creator.user.7bd84e3 create mode 100755 moving_model/plot create mode 100755 moving_model/plot_pdf diff --git a/moving_model/Dramp.c b/moving_model/Dramp.c index 8ab66f6..df64442 100644 --- a/moving_model/Dramp.c +++ b/moving_model/Dramp.c @@ -36,7 +36,7 @@ typedef enum{ static movingsage_t movingstage = STAGE_STOPPED; -int initlims(limits_t *lim){ +static int initlims(limits_t *lim){ if(!lim) return FALSE; Min = lim->min; Max = lim->max; diff --git a/moving_model/Tramp.c b/moving_model/Tramp.c index a86835b..ea26f88 100644 --- a/moving_model/Tramp.c +++ b/moving_model/Tramp.c @@ -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){ diff --git a/moving_model/main.c b/moving_model/main.c index e033cf0..f6b8e6b 100644 --- a/moving_model/main.c +++ b/moving_model/main.c @@ -34,12 +34,12 @@ typedef struct{ static pars G = { .ramptype = "d", - .dT = 100000, + .dT = 10000, }; static limits_t limits = { .min = {.coord = -1e6, .speed = 0.1, .accel = 0.1}, - .max = {.coord = 1e6, .speed = 1e3, .accel = 10.}, + .max = {.coord = 1e6, .speed = 1e3, .accel = 50.}, .jerk = 10. }; @@ -56,6 +56,7 @@ static int move(moveparam_t *tag){ if(!tag) ERRX("move(): needs target"); moveparam_t curpos; movestate_t curstate = model->get_state(&curpos); + if(curstate == ST_MOVE) printf("Current state: moving; recalculte new parameters\n"); if(!move_to(tag)){ WARNX("move(): can't move to %g with max speed %g", tag->coord, tag->speed); return FALSE; @@ -91,7 +92,7 @@ int main(int argc, char **argv){ if(!coordslog) ERR("Can't open %s", G.xlog); } else coordslog = stdout; if(G.dT < 1) G.dT = 1; - fprintf(coordslog, "# time coordinate speed acceleration\n"); + fprintf(coordslog, "time coordinate speed acceleration\n"); ramptype_t ramp = RAMP_AMOUNT; if(*G.ramptype == 'd' || *G.ramptype == 'D') ramp = RAMP_DUMB; else if(*G.ramptype == 't' || *G.ramptype == 'T') ramp = RAMP_TRAPEZIUM; @@ -100,13 +101,22 @@ int main(int argc, char **argv){ model = init_moving(ramp, &limits); if(!model) ERRX("Can't init moving model: check parameters"); Tstart = nanot(); - moveparam_t target = {.speed = 100., .coord = 1000}; + moveparam_t target = {.speed = 10., .coord = 20.}; + if(move(&target)) monit(0.5); + for(int i = 0; i < 10; ++i){ + target.coord = -target.coord; + if(move(&target)) monit(1.); + } + target.coord = 0.; target.speed = 20.; + if(move(&target)) monit(100.); + return 0; + if(move(&target)) monit(1.); + target.speed = 15.; + target.coord = 30.; + if(move(&target)) monit(1.); + target.coord = 0.; if(move(&target)) monit(1.5); - target.speed = 500.; - if(move(&target)) monit(2.5); - target.coord = -100.; - if(move(&target)) monit(4.); - target.coord = 0.; target.speed = -200.; + target.coord = 0.; target.speed = 20.; if(move(&target)) monit(1e6); usleep(5000); fclose(coordslog); diff --git a/moving_model/moving_model.creator.user b/moving_model/moving_model.creator.user index 765e0ab..2c3598d 100644 --- a/moving_model/moving_model.creator.user +++ b/moving_model/moving_model.creator.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/moving_model/moving_model.creator.user.7bd84e3 b/moving_model/moving_model.creator.user.7bd84e3 new file mode 100644 index 0000000..1311ab3 --- /dev/null +++ b/moving_model/moving_model.creator.user.7bd84e3 @@ -0,0 +1,184 @@ + + + + + + EnvironmentId + {7bd84e39-ca37-46d3-be9d-99ebea85bc0d} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + true + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + KOI8-R + false + 4 + false + 0 + 80 + true + true + 1 + 0 + false + true + false + 0 + true + true + 0 + 8 + true + false + 1 + true + false + true + *.md, *.MD, Makefile + false + true + true + + + + ProjectExplorer.Project.PluginSettings + + + true + false + true + true + true + true + + false + + + 0 + true + + true + true + Builtin.DefaultTidyAndClazy + 8 + true + + + + true + + + + + ProjectExplorer.Project.Target.0 + + Desktop + Desktop + Desktop + {65a14f9e-e008-4c1b-89df-4eaa4774b6e3} + 0 + 0 + 0 + + /Big/Data/00__Small_tel/moving_model + + + + all + + true + GenericProjectManager.GenericMakeStep + + 1 + Build + Build + ProjectExplorer.BuildSteps.Build + + + + + clean + + true + GenericProjectManager.GenericMakeStep + + 1 + Clean + Clean + ProjectExplorer.BuildSteps.Clean + + 2 + false + + false + + Default + GenericProjectManager.GenericBuildConfiguration + + 1 + + + 0 + Deploy + Deploy + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + true + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.FileVersion + 22 + + + Version + 22 + + diff --git a/moving_model/plot b/moving_model/plot new file mode 100755 index 0000000..f6c9490 --- /dev/null +++ b/moving_model/plot @@ -0,0 +1,6 @@ +#!/usr/bin/gnuplot + +#set term pdf +#set output "output.pdf" +plot for [col=2:4] 'coordlog' using 1:col with lines title columnheader +pause mouse diff --git a/moving_model/plot_pdf b/moving_model/plot_pdf new file mode 100755 index 0000000..2001ca0 --- /dev/null +++ b/moving_model/plot_pdf @@ -0,0 +1,5 @@ +#!/usr/bin/gnuplot + +set term pdf +set output "output.pdf" +plot for [col=2:4] 'coordlog' using 1:col with lines title columnheader