/* * This file is part of the moving_model project. * Copyright 2025 Edward V. Emelianov . * * 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 . */ // simplest trapezioidal ramp #include #include #include #include #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 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, };