motion profile with limited velocity and acceleration (trapezoidal velocity)

derivslim.pde

/*
 This program is for testing code that can be used to limit the first and second derivative of a variable as it approaches a target value.
 
 It's easiest to think of in terms of position, velocity, and acceleration. If used with a servo, for example, the servo would smoothly move to a target value with a trapezoidal velocity profile.
 
 The formula in this program supports being run at uneven intervals, and allows for editing the target, position, and velocity while it runs
 Arduino library I wrote using the algoritihm developed here: https://github.com/joshua-8/Derivs_Limiter
 
 The code was rewritten for version 3 of Derivs_Limiter (supporting different accel from decel and velocity target mode) in June 2022 by joshua-8
 */
float velLimit=100;  //>0
float accelLimit=200; //>0
float decelLimit=100;
boolean preventGoingWrongWay=false;
boolean preventGoingTooFast=false;
float posLimitLow=Float.NEGATIVE_INFINITY;
float posLimitHigh=Float.POSITIVE_INFINITY; //>=low
float maxStoppingDecel=1; //>=1
boolean posMode=true;


float velocityTarget=0;
float position=500;
float velocity=0;
float accel=0;
long lastTime=0;
float target=900;
float time=0;
float lastTarget=target;
float targetDelta=0;


import grafica.*; //https://github.com/jagracar/grafica
GPointsArray pointsPos = new GPointsArray(1000);
GPointsArray pointsVel = new GPointsArray(1000);
GPointsArray pointsAcc = new GPointsArray(1000);
int gi=0;
GPlot plotPos;
GPlot plotVel;
GPlot plotAcc;
int lastMouseX;

float _calc() {
  time=(millis()-lastTime)/1000.0000;
  if (lastTime==0) {
    time=0; //in case there's a delay between starting the program and the first calculation avoid jump at start
    lastTime=millis();
  }  
  if (time==0) {
    return position;
  }
  lastTime=millis();
  if (position >= posLimitHigh) {
    position = posLimitHigh;
    velocity = 0;
  } else if (position <= posLimitLow) {
    position = posLimitLow;
    velocity = 0;
  }
  target = constrain(target, posLimitLow, posLimitHigh);
  targetDelta = target - lastTarget;
  lastTarget = target;


  if (preventGoingTooFast)
    velocity = constrain(velocity, -velLimit, velLimit);


  if (posMode) {

    if (preventGoingWrongWay && velocity != 0 && target != position && ((velocity > 0) != (target - position > 0))) //prevent going the wrong way
      velocity = 0;

    if (velocity==0&&position==target) { //if stopped at the target, no calculations are needed 
      accel=0;
      println("stopped at target pos");

      return position;
    }
    if (velocity!=0&&target!=position && (velocity > 0) == (target - position > 0)
      && (abs(position-target)-abs(velocity*(time))<=sq(velocity) / 2.0 / decelLimit)) {
      //predicted to be too close next time, decel now.
      if (abs(position-target)<=abs(velocity*time)&&(abs(velocity)<=decelLimit*maxStoppingDecel*time)) {//close enough and slow enough, just stop
        accel=0;
        velocity=0;
        position=target;
        println("there A");
      } else { //decel
        accel = -sq(velocity) / 2.0 / (target - (position));
        print(accel+" ");
        print((target-position)+" ");
        accel = constrain(accel, -decelLimit * maxStoppingDecel, decelLimit * maxStoppingDecel);
        println("decel");
        velocity+=accel*time;
        position+=velocity*time;
      }
    } else if (velocity!=0&&target!=position&&(velocity > 0) != (target - position > 0)) {//if going wrong way, decel
      accel=((target-position>0)?decelLimit:-decelLimit);
      velocity+=accel*time;
      if (velocity!=0&&(velocity > 0) == (target - position > 0)) { //switched direction, stop at zero velocity, incase accel is lower
        velocity=0;
        accel=0;
        println("WW stopped");
      } else {    
        println("wrong way");
        position+=velocity*time;
      }
    } else if (abs(velocity)<velLimit) {//too slow, speed up
      println("TOO SLOW");
      float tempVelocity=velocity;
      accel=(position>target)?-accelLimit:accelLimit;
      velocity+=accel*time;
      velocity=constrain(velocity, -velLimit, velLimit);
      float maxSpeedThatCanBeStopped=sqrt(2*(decelLimit)*abs(position-target)); //v^2 = u^2 + 2as
      velocity=constrain(velocity, -maxSpeedThatCanBeStopped, maxSpeedThatCanBeStopped);
      accel=(velocity-tempVelocity)/time;
      position+=velocity*time;
      if (abs(position-target)<=abs(velocity*time)&&(abs(velocity)<=decelLimit*maxStoppingDecel*time)) {//close enough and slow enough, just stop
        accel=0;
        velocity=0;
        position=target;
        println("there t s");
      }
    } else if (abs(velocity)>velLimit) {//too fast, slow down
      println("slow down");
      boolean velPositive=(velocity>0);
      float tempVelocity=velocity;
      velocity+=velPositive ? -decelLimit*time : decelLimit*time;
      if (velPositive) {
        if (velocity<velLimit) {
          velocity=velLimit;
        }
      } else {//vel negative
        if (velocity>-velLimit) {
          velocity=-velLimit;
        }
      }

      accel=(velocity-tempVelocity)/time;
      position+=velocity*time;
    } else { //coast, no accel
      accel=0;
      position+=velocity*time;
    }
  } else {//velmode
    float tempVelocity = velocity;
    velocityTarget=constrain(velocityTarget, -velLimit, velLimit);
    if (preventGoingWrongWay&&velocity!=0&&velocityTarget!=0&&velocity>0!=velocityTarget>0) {
      velocity=0;
    }
    if (velocity != velocityTarget) {
      if (velocity == 0) {
        velocity += constrain(velocityTarget - velocity, -accelLimit * time, accelLimit * time);
      } else if (velocity > 0) {
        velocity += constrain(velocityTarget - velocity, -decelLimit * time, accelLimit * time);
        if (velocity<0) { //prevent decel from crossing zero and causing accel
          velocity=0;
        }
      } else { //velocity < 0
        velocity += constrain(velocityTarget - velocity, -accelLimit * time, decelLimit * time);
        if (velocity>0) { //prevent decel from crossing zero and causing accel
          velocity=0;
        }
      }
    }    
    print(velocityTarget);
    print(",");
    println(velocity);
    accel = (velocity - tempVelocity) / time;
    position += velocity * time;
  }

  return position;
}

boolean setVelLimitForTimedMove(float _dist, float _time) {
  _dist=abs(_dist);
  _time=abs(_time);
  float tempVelLimit;
  if (accelLimit==Float.POSITIVE_INFINITY)
    tempVelLimit=_dist/_time;
  else
    tempVelLimit=(-0.5*accelLimit*(-_time+sqrt(sq(_time)-4*_dist/accelLimit)));
  boolean possible=(tempVelLimit==tempVelLimit);//nan check
  if (possible) {
    velLimit=tempVelLimit;
  }
  return possible;
}

boolean setTargetAndVelLimitForTimedMove(float _target, float _time) {
  boolean ret=setVelLimitForTimedMove(_target-position, _time);
  if (ret)
    target=_target;
  return ret;
}



void setup() {

  plotPos = new GPlot(this, 0, 125, 1000, 400);
  plotVel = new GPlot(this, 0, 125+400, 1000, 400);
  plotAcc = new GPlot(this, 0, 124+2*400, 1000, 400);

  size(1000, 1325);
  background(0);
  noStroke();
  fill(255);
  frameRate(120);
}
void draw() {

  background(0);

  _calc();



  if (keyPressed&&key=='r') {
    velocity=mouseX-500;
  }
  if (keyPressed&&key=='v') {
    posMode=false;
    velocityTarget=mouseX-500;
  }
  if (keyPressed&&key=='t') {
    velocity=(mouseX-500)*10000.0;
  }
  if (mousePressed&&mouseButton==RIGHT) {

    //    position=mouseX;
    //  velocity=mouseX-lastMouseX;
    //    velocity=mouseX-lastMouseX;
    //velocity/=time;
    target=mouseX;

    posMode=true;
  }
  if (mousePressed&&mouseButton==CENTER) {
    position=mouseX;
    //velocity=mouseX-lastMouseX;
    //velocity/=time;

    posMode=true;
  }
  if (mousePressed&&mouseButton==LEFT) {
    //target=mouseX;
    position=mouseX;
    velocity=mouseX-lastMouseX;
    velocity/=time;

    //posMode=true;


    //setTargetAndVelLimitForTimedMove(mouseX, 5);
    //jogPos(mouseX-width/2);
  }
  if (keyPressed&&key==BACKSPACE) {
    pointsPos= new GPointsArray(1000);
    pointsVel= new GPointsArray(1000);
    pointsAcc= new GPointsArray(1000);
  }
  lastMouseX=mouseX;


  //delay(100);
  delay(int(random(20))); //create unstable loop time

  pointsPos.add(lastTime, position);
  pointsVel.add(lastTime, velocity);
  pointsAcc.add(lastTime, accel);
  gi++;

  fill(255);

  circle(position, 60, 100);

  fill(0, 255, 0, 100);
  circle(target, 60, 100);


  plotPos.setPoints(pointsPos);
  plotPos.defaultDraw();
  plotVel.setPoints(pointsVel);
  plotVel.defaultDraw();
  plotAcc.setPoints(pointsAcc);
  plotAcc.defaultDraw();
}

updated for Derivs_Limiter v2

now updated for Derivs_Limiter v3
https://github.com/joshua-8/derivs_limiter