mountcontrol/moving_model/Tramp.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 <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

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,
};