diff --git a/moving_model/Dramp.c b/moving_model/Dramp.c index 18b9a1b..8ab66f6 100644 --- a/moving_model/Dramp.c +++ b/moving_model/Dramp.c @@ -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; diff --git a/moving_model/Tramp.c b/moving_model/Tramp.c index a2748b4..7711da8 100644 --- a/moving_model/Tramp.c +++ b/moving_model/Tramp.c @@ -18,12 +18,229 @@ // 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 -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, +}; diff --git a/moving_model/moving.c b/moving_model/moving.c index b80670e..41f6cde 100644 --- a/moving_model/moving.c +++ b/moving_model/moving.c @@ -16,6 +16,7 @@ * along with this program. If not, see . */ +#include #include #include #include @@ -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); diff --git a/moving_model/moving.h b/moving_model/moving.h index eb96419..a0f6d62 100644 --- a/moving_model/moving.h +++ b/moving_model/moving.h @@ -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); diff --git a/moving_model/moving_model.creator.user b/moving_model/moving_model.creator.user index 1311ab3..ac56112 100644 --- a/moving_model/moving_model.creator.user +++ b/moving_model/moving_model.creator.user @@ -1,10 +1,10 @@ - + EnvironmentId - {7bd84e39-ca37-46d3-be9d-99ebea85bc0d} + {cf63021e-ef53-49b0-b03b-2f2570cdf3b6} ProjectExplorer.Project.ActiveTarget @@ -13,8 +13,8 @@ ProjectExplorer.Project.EditorSettings - true true + false true Cpp @@ -42,7 +42,7 @@ false true false - 0 + 1 true true 0 @@ -79,7 +79,7 @@ true true Builtin.DefaultTidyAndClazy - 8 + 1 true @@ -94,12 +94,12 @@ Desktop Desktop Desktop - {65a14f9e-e008-4c1b-89df-4eaa4774b6e3} + {91347f2c-5221-46a7-80b1-0a054ca02f79} 0 0 0 - /Big/Data/00__Small_tel/moving_model + /home/eddy/C-files/mountcontrol.git/moving_model @@ -109,8 +109,8 @@ GenericProjectManager.GenericMakeStep 1 - Build - Build + Сборка + Сборка ProjectExplorer.BuildSteps.Build @@ -122,8 +122,8 @@ GenericProjectManager.GenericMakeStep 1 - Clean - Clean + Очистка + Очистка ProjectExplorer.BuildSteps.Clean 2 @@ -131,15 +131,15 @@ false - Default + По умолчанию GenericProjectManager.GenericBuildConfiguration 1 0 - Deploy - Deploy + Развёртывание + Развёртывание ProjectExplorer.BuildSteps.Deploy 1 @@ -153,6 +153,7 @@ true 0 true + true 2