add time based movements

README.md

@@ -11,6 +11,8 @@ This library can be used to limit the first and second derivative of a variable
 It's easiest to think of in terms of position, velocity, and acceleration. 
 If used with a servo, for example, the servo will smoothly move to a target value with a trapezoidal velocity profile.
 
+You can also specify how long you want a move to take and the library can calculate the velocity needed to make that happen.
+
 The formula in this program supports being run at uneven intervals, and allows for editing the target, position, and velocity while it runs since it doesn't rely on calculating a motion profile ahead of time.
 
 ## Usage:
@@ -101,7 +103,13 @@ _**Velocity, position and/or acceleration limits can be set as INFINITY in order
   `  bool isPosNotAtTarget()`  
 
   `  float distToTarget()`  
-
+
+  `  void resetVelLimitToOriginal()`
+
+  `  boolean setVelLimitForTimedMove(float _dist, float _time, float _maxVel = NAN)`
+
+  `  boolean setTargetAndVelLimitForTimedMove(float _target, float _time, float _maxVel = NAN)`
+
 
 ## Notes:
 

examples/AllFunctions/AllFunctions.ino

@@ -3,7 +3,7 @@
  *  https://github.com/joshua-8/Derivs_Limiter
  * 
  *  This example shows every function that the library has, 
- *  while it should compile it doesn't really do anything.
+ *  while it should compile, it is not meant to actually be run
  * 
  *  A more proper reference is here: https://joshua-8.github.io/Derivs_Limiter/html/class_derivs___limiter.html
  */
@@ -62,5 +62,8 @@ void loop()
     limiter.setVelocityPointer(&vel);
     float pos;
     limiter.setPositionPointer(&pos);
-    limiter.setPosition(NAN); //has no effect
+    limiter.setPosition(NAN); //has no effect because NAN
+    limiter.setVelLimitForTimedMove(180, 10, 100); //dist, time, maxVel
+    limiter.setTargetAndVelLimitForTimedMove(180, 10, 100); //target, time, maxVel
+    limiter.resetVelLimitToOriginal();
 }

examples/SmoothServoTimeBased/SmoothServoTimeBased.ino

@@ -0,0 +1,42 @@
+/**
+ * SmoothServoTimeBased, an example for the Derivs_Limiter library
+ * https://github.com/joshua-8/Derivs_Limiter
+ * 
+ * Connect a servo to power and pin 9,
+ * and/or open the serial plotter to see the smoothed position values.
+ * Every 15 seconds the servo should move for 10 seconds.
+ */
+#include <Arduino.h>
+#include <Derivs_Limiter.h>
+#include <Servo.h>
+
+Derivs_Limiter limiter = Derivs_Limiter(100, 75); // velocityLimit, accelerationLimit
+Servo myServo;
+boolean state = true;
+void setup()
+{
+    Serial.begin(9600);
+    myServo.attach(9);
+}
+void loop()
+{
+    if (millis() % 30000 < 15000) { //toggles every 15 seconds
+        if (state == true)
+            limiter.setTargetAndVelLimitForTimedMove(0, 10);
+        state = false;
+        myServo.write(limiter.calc());
+        limiter.calc();
+    } else {
+        if (state == false)
+            limiter.setTargetAndVelLimitForTimedMove(180, 10);
+        state = true;
+        myServo.write(limiter.calc());
+        limiter.calc();
+    }
+    Serial.print(limiter.getPosition());
+    Serial.print(",");
+    Serial.print(limiter.getVelocity());
+    Serial.print(",");
+    Serial.print(limiter.getAcceleration());
+    Serial.println();
+}

library.properties

@@ -1,5 +1,5 @@
 name=Derivs_Limiter
-version=2.5.0
+version=2.6.0
 author=Joshua Phelps <[email protected]>
 maintainer=Joshua Phelps <[email protected]>
 sentence=This library can be used to limit the first and second derivative of a variable as it approaches a target value.

src/Derivs_Limiter.h

@@ -24,6 +24,7 @@ class Derivs_Limiter {
     float targetDelta;
     float lastPos;
     float posDelta;
+    float originalVelLimit;
     float* positionPointer;
     float* velocityPointer;
 
@@ -51,6 +52,7 @@ class Derivs_Limiter {
         accel = 0;
         lastTime = 0;
         velLimit = abs(_velLimit);
+        originalVelLimit = velLimit;
         accelLimit = abs(_accelLimit);
         target = 0;
         if (!isnan(_target))
@@ -126,6 +128,7 @@ class Derivs_Limiter {
     {
         if (velLim != velLimit) {
             velLimit = abs(velLim);
+            originalVelLimit = velLimit;
             return true;
         }
         return false;
@@ -478,6 +481,57 @@ class Derivs_Limiter {
         return target - position;
     }
 
+    /**
+     * @brief  resets the velocity limit to the value set in the constructor or setVelLimit()
+     * @note may be useful, since setTargetAndVelLimitForTimedMove and setVelLimitForTimedMove change the velocity limit
+     */
+    void resetVelLimitToOriginal()
+    {
+        velLimit = originalVelLimit;
+    }
+
+    /**
+     * @brief  This function changes velLimit so that a move of a specified distance takes the specified time (if possible given acceleration limit)
+     * @note   using this function changes the value of velLimit from whatever you set it to when you created the Derivs_Limiter object
+     * @param  _dist: (float) how far you want to move
+     * @param  _time: (float) time in seconds that you would like it to take to move the given distance
+     * @param  _maxVel: (float, optional, default=NAN) maximum allowable velocity, if the required velocity exceeds this the function returns false, if NAN the velocity limit set in the constructor or setVelLimit() is used
+     * @retval (bool) true if move possible within time given acceleration limit, false if not possible (and nothing is changed)
+     */
+    boolean setVelLimitForTimedMove(float _dist, float _time, float _maxVel = NAN)
+    {
+        _dist = abs(_dist);
+        _time = abs(_time);
+        if (isnan(_maxVel))
+            _maxVel = originalVelLimit;
+        float tempVelLimit;
+        if (accelLimit == INFINITY)
+            tempVelLimit = _dist / _time;
+        else
+            tempVelLimit = (-0.5 * accelLimit * (-_time + sqrt(sq(_time) - 4 * _dist / accelLimit)));
+        boolean possible = !isnan(tempVelLimit) && tempVelLimit <= abs(_maxVel); //nan check, speed check
+        if (possible) {
+            velLimit = tempVelLimit;
+        }
+        return possible;
+    }
+
+    /**
+     * @brief  This function changes velLimit so that a move to the specified target position takes the specified time (if possible given acceleration limit) use like setTarget()
+     * @note   using this function changes the value of velLimit from whatever you set it to when you created the Derivs_Limiter object
+     * @param  _target: (float) position you'd like to move to
+     * @param  _time: (float) how long you would like the movement to take
+     * @param  _maxVel: (float, optional, default=NAN) maximum allowable velocity, if the required velocity exceeds this the function returns false, if NAN the velocity limit set in the constructor or setVelLimit() is used
+     * @retval (bool) true if move possible within time given acceleration limit, false if not possible (and nothing is changed)
+     */
+    boolean setTargetAndVelLimitForTimedMove(float _target, float _time, float _maxVel = NAN)
+    {
+        boolean ret = setVelLimitForTimedMove(_target - position, _time, _maxVel);
+        if (ret)
+            target = _target;
+        return ret;
+    }
+
 protected:
     /**
      * @brief  this is where the actual code is
@@ -517,8 +571,7 @@ class Derivs_Limiter {
 
         if (preventGoingTooFast)
             velocity = constrain(velocity, -velLimit, velLimit);
-
-        if ((target == position && (preventGoingWrongWay || velocity == 0)) || ((velocity != 0 && target != position && (velocity > 0) == (target - position > 0)) && abs(target - position) <= abs(velocity * time) && (abs(velocity) < accelLimit * time * maxStoppingAccel))) { //basically there
+        if ((target == position && (preventGoingWrongWay || velocity == 0 || (abs(velocity) < accelLimit * time * maxStoppingAccel))) || ((velocity != 0 && target != position && (velocity > 0) == (target - position > 0)) && abs(target - position) <= abs(velocity * time) && (abs(velocity) < accelLimit * time * maxStoppingAccel))) { //basically there
             accel = 0;
             velocity = 0;
             position = target;
@@ -527,7 +580,7 @@ class Derivs_Limiter {
             accel = (((target - position) > 0 ? velLimit : -velLimit) - velocity) / time; //acceleration to reach target vel
             accel = constrain(accel, -maxAccelA, maxAccelA);
             if (velocity == 0 || (velocity > 0) == (target - position > 0)) { //going towards target
-                float maxAccelB = abs((target - position) / time / time);
+                float maxAccelB = (abs(target - position) / time - abs(velocity)) / time;
                 accel = constrain(accel, -maxAccelB, maxAccelB); //don't overshoot
             }
 
@@ -541,6 +594,16 @@ class Derivs_Limiter {
                     accel = constrain(accel, -accelLimit * maxStoppingAccel, accelLimit * maxStoppingAccel);
                 }
             }
+            if (abs(accel) == INFINITY) {
+                accel = 0;
+                velocity = 0;
+                position = target;
+            }
+            if (((velocity != 0 && target != position && (velocity > 0) == (target - position > 0)) && abs(target - position) <= abs(velocity * time) && (abs(velocity) < accelLimit * time * maxStoppingAccel))) { //final check against overshoot
+                accel = 0;
+                velocity = 0;
+                position = target;
+            }
             velocity += accel * time;
             position += velocity * time;
         }