Derivs Limiter
Loading...
Searching...
No Matches
Derivs_Limiter.h
Go to the documentation of this file.
1#ifndef _DERIVS_LIMITER_H_
2#define _DERIVS_LIMITER_H_
3#include <Arduino.h>
9protected:
10 float position;
11 float velocity;
12 float accel;
13 unsigned long lastTime;
14 float target;
15 float velLimit;
18 float time;
26 float lastPos;
27 float posDelta;
31 bool posMode;
33
34public:
51 Derivs_Limiter(float _velLimit, float _accelLimit, float _decelLimit = NAN, float _target = 0,
52 float _startPos = 0, float _startVel = 0, bool _preventGoingWrongWay = false, bool _preventGoingTooFast = false,
53 float _posLimitLow = -INFINITY, float _posLimitHigh = INFINITY, float _maxStoppingDecel = 2,
54 float* _posPointer = NULL, float* _velPointer = NULL)
55 {
56 accel = 0;
57 lastTime = 0;
58 velLimit = abs(_velLimit);
60 accelLimit = abs(_accelLimit);
61 setDecelLimit(_decelLimit);
62 target = 0;
63 if (!isnan(_target))
64 target = _target;
65 posMode = true;
66 lastTarget = _target;
67 targetDelta = 0;
68 position = 0;
69 if (!isnan(_startPos))
70 position = _startPos;
72 posDelta = 0;
73 velocity = 0;
74 if (!isnan(_startVel))
75 velocity = _startVel;
76 time = 0;
77 preventGoingWrongWay = _preventGoingWrongWay;
78 preventGoingTooFast = _preventGoingTooFast;
79 posLimitLow = _posLimitLow;
80 posLimitHigh = max(_posLimitHigh, posLimitLow);
81 maxStoppingDecel = max(_maxStoppingDecel, (float)1.0);
82 positionPointer = _posPointer;
83 velocityPointer = _velPointer;
85 }
86
92 {
93 accel = 0;
94 lastTime = 0;
95 velLimit = 0;
97 accelLimit = 0;
98 setDecelLimit(NAN);
99 target = 0;
100 posMode = true;
101 lastTarget = 0;
102 targetDelta = 0;
103 position = 0;
104 lastPos = 0;
105 posDelta = 0;
106 velocity = 0;
107 time = 0;
108 preventGoingWrongWay = false;
109 preventGoingTooFast = false;
110 posLimitLow = 0;
111 posLimitHigh = 0;
113 positionPointer = NULL;
114 velocityPointer = NULL;
115 velocityTarget = 0;
116 }
117
124 void setPositionVelocity(float pos = 0, float vel = 0)
125 {
126 if (!isnan(pos))
127 position = pos;
128 if (!isnan(vel))
129 velocity = vel;
130 }
131
138 void setTargetAndPosition(float targ = 0, float pos = 0)
139 {
140 if (!isnan(targ)) {
141 target = targ;
142 posMode = true;
143 }
144 if (!isnan(pos))
145 position = pos;
146 }
147
153 bool setPosition(float pos = 0)
154 {
155 if (pos != position) {
156 if (!isnan(pos)) {
157 position = pos;
158 return true;
159 }
160 }
161 return false;
162 }
163
170 bool setVelocity(float vel = 0)
171 {
172 if (vel != velocity) {
173 if (!isnan(vel)) {
174 velocity = vel;
175 return true;
176 }
177 }
178 return false;
179 }
180
186 bool setVelLimit(float velLim)
187 {
188 if (velLim != velLimit) {
189 velLimit = abs(velLim);
191 return true;
192 }
193 return false;
194 }
195
201 bool setAccelLimit(float accelLim)
202 {
203 if (accelLim != accelLimit) {
204 accelLimit = abs(accelLim);
205 return true;
206 }
207 return false;
208 }
209
215 void setDecelLimit(float _decelLimit = NAN)
216 {
217 if (isnan(_decelLimit)) { // decelLimit defaults to accelLimit
219 } else {
220 decelLimit = abs(_decelLimit);
221 }
222 }
223
230 void setAccelAndDecelLimits(float _accelLimit, float _decelLimit = NAN)
231 {
232 setAccelLimit(_accelLimit);
233 setDecelLimit(_decelLimit);
234 }
235
243 void setVelAccelLimits(float velLim, float accLim, float decLim = NAN)
244 {
245 setVelLimit(velLim);
246 setAccelLimit(accLim);
247 setDecelLimit(decLim);
248 }
249
255 {
256 return velLimit;
257 }
258
264 {
265 return accelLimit;
266 }
267
273 {
274 return decelLimit;
275 }
276
282 {
283 return velocity;
284 }
285
292 {
293 return accel;
294 }
295
301 {
302 return position;
303 }
304
310 void setMaxStoppingDecel(float _maxStoppingDecel)
311 {
312 maxStoppingDecel = max(_maxStoppingDecel, (float)1.0);
313 }
314
320 {
321 return maxStoppingDecel;
322 }
323
329 {
330 return posLimitLow;
331 }
332
338 {
339 return posLimitHigh;
340 }
341
348 bool setLowPosLimit(float lowLimit)
349 {
350 if (lowLimit < posLimitHigh) {
351 posLimitLow = lowLimit;
352 return true;
353 }
354 return false;
355 }
356
363 bool setHighPosLimit(float highLimit)
364 {
365 if (highLimit > posLimitLow) {
366 posLimitHigh = highLimit;
367 return true;
368 }
369 return false;
370 }
371
378 void setPosLimits(float lowLimit, float highLimit)
379 {
380 setLowPosLimit(lowLimit);
381 setHighPosLimit(highLimit);
382 }
383
389 bool setTarget(float _target)
390 {
391 if (!isnan(_target)) {
392 target = _target;
393 posMode = true;
394 }
395 return position == target;
396 }
397
402 float getTarget()
403 {
404 return target;
405 }
406
412 void setPositionAndTarget(float targPos)
413 {
414 setPosition(targPos);
415 setTarget(targPos);
416 }
417
423 void jogPosition(float increment)
424 {
425 velocity = 0;
426 setPositionAndTarget(position + increment);
427 }
428
434 {
435 lastTime = micros();
436 }
437
442 unsigned long getLastTime()
443 {
444 return lastTime;
445 }
446
452 {
453 return time;
454 }
455
461 {
462 return targetDelta;
463 }
464
470 {
471 return lastTarget;
472 }
473
479 {
480 return posDelta;
481 }
482
488 {
489 return lastPos;
490 }
491
498 {
499 if (time > 0)
500 return targetDelta / time;
501 return 0;
502 }
503
510 void setPositionPointer(float* _positionPointer)
511 {
512 positionPointer = _positionPointer;
513 }
514
521 void setVelocityPointer(float* _velocityPointer)
522 {
523 velocityPointer = _velocityPointer;
524 }
525
531 void setPreventGoingWrongWay(bool _preventGoingWrongWay)
532 {
533 preventGoingWrongWay = _preventGoingWrongWay;
534 }
535
541 {
543 }
544
550 void setPreventGoingTooFast(bool _preventGoingTooFast)
551 {
552 preventGoingTooFast = _preventGoingTooFast;
553 }
554
560 {
561 return preventGoingTooFast;
562 }
563
569 {
570 return position == target;
571 }
572
578 {
579 return position != target;
580 }
581
588 {
589 return target - position;
590 }
591
597 void setVelConstant(float vel)
598 {
599 if (isnan(vel)) {
600 return;
601 }
602 posMode = false;
603 velocity = vel;
604 velocityTarget = vel;
605 }
606
612 void setVelTarget(float vel)
613 {
614 if (isnan(vel)) {
615 return;
616 }
617 posMode = false;
618 velocityTarget = vel;
619 }
620
626 {
627 return posMode;
628 }
629
635 {
636 return velocityTarget;
637 }
638
648
657 boolean setVelLimitForTimedMove(float _dist, float _time, float _maxVel = NAN)
658 {
659 if (isnan(_dist) || isnan(_time)) {
660 return false;
661 }
662 _dist = abs(_dist);
663 _time = abs(_time);
664 if (isnan(_maxVel))
665 _maxVel = originalVelLimit;
666 float tempVelLimit;
667 if (accelLimit == INFINITY && decelLimit == INFINITY)
668 tempVelLimit = _dist / _time;
669 else {
670 float acc;
671 if (accelLimit == INFINITY && decelLimit != INFINITY)
672 acc = decelLimit * 2;
673 else if (accelLimit != INFINITY && decelLimit == INFINITY)
674 acc = accelLimit * 2;
675 else
676 acc = sqrt(decelLimit * accelLimit); // find single acceleration that takes equivalent time to the two different limits.
677 tempVelLimit = (-0.5 * acc * (-_time + sqrt(sq(_time) - 4 * _dist / acc)));
678 }
679 boolean possible = !isnan(tempVelLimit) && tempVelLimit <= abs(_maxVel); // nan check, speed check
680 if (possible) {
681 velLimit = tempVelLimit;
682 }
683 return possible;
684 }
685
694 boolean setTargetAndVelLimitForTimedMove(float _target, float _time, float _maxVel = NAN)
695 {
696 boolean ret = setVelLimitForTimedMove(_target - position, _time, _maxVel);
697 if (ret) {
698 target = _target;
699 posMode = true;
700 }
701 return ret;
702 }
703
712 boolean setTargetTimedMovePreferred(float _target, float _time, float _maxVel = NAN)
713 {
714 boolean ret = setVelLimitForTimedMove(_target - position, _time, _maxVel);
715 if (ret) {
716 target = _target;
717 posMode = true;
718 } else { // not possible in time given acceleration
719 if (isnan(_maxVel))
721 else
722 velLimit = _maxVel;
723 target = _target;
724 posMode = true;
725 }
726 return ret;
727 }
728
733 float calc()
734 {
735 return _calc();
736 }
737
743 float calc(float _target)
744 {
745 if (!isnan(_target)) {
746 target = _target;
747 posMode = true;
748 }
749 return _calc();
750 }
751
752protected:
757 virtual float _calc()
758 {
759 if (positionPointer && !isnan(*positionPointer))
761
762 if (velocityPointer && !isnan(*velocityPointer))
764
765 time = (micros() - lastTime) / 1000000.0;
766 if (lastTime == 0) {
767 time = 0; // in case there's a delay between starting the program and the first calculation avoid jump at start
768 lastTime = micros();
769 }
770 if (time == 0) {
771 return position;
772 }
773 lastTime = micros();
774
775 // constrain positions within limits
776 if (position > posLimitHigh) {
778 velocity = 0;
779 } else if (position < posLimitLow) {
781 velocity = 0;
782 }
783 target = constrain(target, posLimitLow, posLimitHigh);
784
787
789 velocity = constrain(velocity, -velLimit, velLimit);
790 }
791 if (posMode) {
792 if (preventGoingWrongWay && velocity != 0 && target != position && ((velocity > 0) != (target - position > 0))) { // going the wrong way
793 velocity = 0;
794 }
795
796 if (velocity == 0 && position == target) { // if stopped at the target, no calculations are needed
797 accel = 0;
798 return position;
799 }
800
801 if (velocity != 0 && target != position && (velocity > 0) == (target - position > 0)
802 && (abs(position - target) - abs(velocity * (time)) <= sq(velocity) / 2.0 / decelLimit)) {
803 // predicted to be too close next time, decel now.
804 if (abs(position - target) <= abs(velocity * time) && (abs(velocity) <= decelLimit * maxStoppingDecel * time)) { // close enough and slow enough, just stop
805 accel = 0;
806 velocity = 0;
808 } else { // decel
809 accel = -sq(velocity) / 2.0 / (target - (position));
811 velocity += accel * time;
813 }
814 } else if (velocity != 0 && target != position && (velocity > 0) != (target - position > 0)) { // if going wrong way, decel
815 accel = ((target - position > 0) ? decelLimit : -decelLimit);
816 velocity += accel * time;
817 if (velocity != 0 && (velocity > 0) == (target - position > 0)) { // switched direction, stop at zero velocity, in case accel is lower
818 velocity = 0;
819 accel = 0;
820 } else {
822 }
823 } else if (abs(velocity) < velLimit) { // too slow, speed up
824 float tempVelocity = velocity;
826 velocity += accel * time;
827 velocity = constrain(velocity, -velLimit, velLimit);
828 float maxSpeedThatCanBeStopped = sqrt(2 * (decelLimit)*abs(position - target)); // v^2 = u^2 + 2as
829 velocity = constrain(velocity, -maxSpeedThatCanBeStopped, maxSpeedThatCanBeStopped);
830 accel = (velocity - tempVelocity) / time;
832 if (abs(position - target) <= abs(velocity * time) && (abs(velocity) <= decelLimit * maxStoppingDecel * time)) { // close enough and slow enough, just stop
833 accel = 0;
834 velocity = 0;
836 }
837 } else if (abs(velocity) > velLimit) { // too fast, slow down
838 boolean velPositive = (velocity > 0);
839 float tempVelocity = velocity;
840 velocity += velPositive ? -decelLimit * time : decelLimit * time;
841 if (velPositive) {
842 if (velocity < velLimit) {
844 }
845 } else { // vel negative
846 if (velocity > -velLimit) {
848 }
849 }
850
851 accel = (velocity - tempVelocity) / time;
853 } else { // coast, no accel
854 accel = 0;
856 }
857 } else { // not pos mode, vel mode
858 float tempVelocity = velocity;
860 if (preventGoingWrongWay && velocity != 0 && velocityTarget != 0 && (velocity > 0) != (velocityTarget > 0)) {
861 velocity = 0;
862 }
863 if (velocity != velocityTarget) {
864 if (velocity == 0) {
866 } else if (velocity > 0) {
868 if (velocity < 0) { // prevent decel from crossing zero and causing accel
869 velocity = 0;
870 }
871 } else { // velocity < 0
873 if (velocity > 0) { // prevent decel from crossing zero and causing accel
874 velocity = 0;
875 }
876 }
877 }
878 accel = (velocity - tempVelocity) / time;
880 }
881
882 if (positionPointer)
884 if (velocityPointer)
886
889
890 return position;
891 }
892};
893#endif
This library can be used to limit the first and second derivative of a variable as it approaches a ta...
Definition Derivs_Limiter.h:8
boolean setVelLimitForTimedMove(float _dist, float _time, float _maxVel=NAN)
This function changes velLimit so that a move of a specified distance takes the specified time (if po...
Definition Derivs_Limiter.h:657
float getVelTarget()
get the target velocity used by the velocity control mode
Definition Derivs_Limiter.h:634
void setMaxStoppingDecel(float _maxStoppingDecel)
set setting for how many times accelLimit can be used to stop in time for target position
Definition Derivs_Limiter.h:310
float position
Definition Derivs_Limiter.h:10
float originalVelLimit
Definition Derivs_Limiter.h:28
void setDecelLimit(float _decelLimit=NAN)
set deceleration limit
Definition Derivs_Limiter.h:215
void resetTime()
If calc hasn't been run for a while, use this before starting to use it again to protect from large j...
Definition Derivs_Limiter.h:433
virtual float _calc()
this is where the actual code is
Definition Derivs_Limiter.h:757
float posDelta
Definition Derivs_Limiter.h:27
float calc(float _target)
call this as frequently as possible to calculate all the values
Definition Derivs_Limiter.h:743
float distToTarget()
returns target - position
Definition Derivs_Limiter.h:587
float getHighPosLimit()
get the higher boundary for position
Definition Derivs_Limiter.h:337
float accel
Definition Derivs_Limiter.h:12
float velocity
Definition Derivs_Limiter.h:11
void setPositionVelocity(float pos=0, float vel=0)
set position and velocity
Definition Derivs_Limiter.h:124
bool setAccelLimit(float accelLim)
set acceleration limit
Definition Derivs_Limiter.h:201
float getTarget()
get target position
Definition Derivs_Limiter.h:402
void setPreventGoingWrongWay(bool _preventGoingWrongWay)
sets value of preventGoingWrongWay, true = immediately set velocity to zero if moving away from targe...
Definition Derivs_Limiter.h:531
float targetDelta
Definition Derivs_Limiter.h:25
float time
Definition Derivs_Limiter.h:18
void setPosLimits(float lowLimit, float highLimit)
set the boundaries for position
Definition Derivs_Limiter.h:378
bool preventGoingWrongWay
Definition Derivs_Limiter.h:19
void setVelConstant(float vel)
switch to velocity mode, and set velocity immediately to a constant value.
Definition Derivs_Limiter.h:597
float lastTarget
Definition Derivs_Limiter.h:24
void jogPosition(float increment)
set position and target to position + increment
Definition Derivs_Limiter.h:423
bool preventGoingTooFast
Definition Derivs_Limiter.h:20
float velLimit
Definition Derivs_Limiter.h:15
Derivs_Limiter(float _velLimit, float _accelLimit, float _decelLimit=NAN, float _target=0, float _startPos=0, float _startVel=0, bool _preventGoingWrongWay=false, bool _preventGoingTooFast=false, float _posLimitLow=-INFINITY, float _posLimitHigh=INFINITY, float _maxStoppingDecel=2, float *_posPointer=NULL, float *_velPointer=NULL)
constructor for Derivs_Limiter class
Definition Derivs_Limiter.h:51
float velocityTarget
Definition Derivs_Limiter.h:32
bool setLowPosLimit(float lowLimit)
set the lower boundary for position
Definition Derivs_Limiter.h:348
float getTargetDeltaPerTime()
how fast was target changing (distance/time)
Definition Derivs_Limiter.h:497
float lastPos
Definition Derivs_Limiter.h:26
bool getPreventGoingTooFast()
returns value of preventGoingTooFast setting
Definition Derivs_Limiter.h:559
bool isPosNotAtTarget()
is position not equal to target?
Definition Derivs_Limiter.h:577
float getLastPosition()
what was position in the most recent run of calc(), can be used to see if position was changed outsid...
Definition Derivs_Limiter.h:487
bool getPreventGoingWrongWay()
returns value of preventGoingWrongWay setting
Definition Derivs_Limiter.h:540
float getPositionDelta()
returns the change in position from the most recent run of calc()
Definition Derivs_Limiter.h:478
float getLastTarget()
what was target in the most recent run of calc()
Definition Derivs_Limiter.h:469
void setPositionPointer(float *_positionPointer)
set pointer to an external variable that will be read and modified during calc as position
Definition Derivs_Limiter.h:510
boolean setTargetTimedMovePreferred(float _target, float _time, float _maxVel=NAN)
This function changes velLimit so that a move to the specified target position takes the specified ti...
Definition Derivs_Limiter.h:712
bool isPosAtTarget()
does position equal target?
Definition Derivs_Limiter.h:568
float getAccelLimit()
get acceleration limit setting
Definition Derivs_Limiter.h:263
void resetVelLimitToOriginal()
resets the velocity limit to the value set in the constructor or setVelLimit()
Definition Derivs_Limiter.h:644
void setPositionAndTarget(float targPos)
set position and target to a value
Definition Derivs_Limiter.h:412
bool setPosition(float pos=0)
set position, ignored if NAN
Definition Derivs_Limiter.h:153
bool setVelLimit(float velLim)
set velocity limit
Definition Derivs_Limiter.h:186
void setVelAccelLimits(float velLim, float accLim, float decLim=NAN)
set velocity and acceleration limits
Definition Derivs_Limiter.h:243
float * velocityPointer
Definition Derivs_Limiter.h:30
bool posMode
Definition Derivs_Limiter.h:31
float maxStoppingDecel
Definition Derivs_Limiter.h:23
Derivs_Limiter()
default constructor for Derivs_Limiter
Definition Derivs_Limiter.h:91
float accelLimit
Definition Derivs_Limiter.h:16
float target
Definition Derivs_Limiter.h:14
bool setHighPosLimit(float highLimit)
set the higher boundary for position
Definition Derivs_Limiter.h:363
unsigned long getLastTime()
returns the value of micros() when calc() last ran
Definition Derivs_Limiter.h:442
void setVelocityPointer(float *_velocityPointer)
set pointer to an external variable that will be read and modified during calc as velocity
Definition Derivs_Limiter.h:521
float getTargetDelta()
returns the change in target from the most recent run of calc()
Definition Derivs_Limiter.h:460
float calc()
call this as frequently as possible to calculate all the values
Definition Derivs_Limiter.h:733
bool setTarget(float _target)
set target position (doesn't run calculation, make sure to run calc() yourself)
Definition Derivs_Limiter.h:389
boolean setTargetAndVelLimitForTimedMove(float _target, float _time, float _maxVel=NAN)
This function changes velLimit so that a move to the specified target position takes the specified ti...
Definition Derivs_Limiter.h:694
float getVelocity()
get the current velocity
Definition Derivs_Limiter.h:281
void setVelTarget(float vel)
switch to velocity mode, and set a target velocity that the target should go towards limited by accel...
Definition Derivs_Limiter.h:612
float getVelLimit()
get velocity limit setting
Definition Derivs_Limiter.h:254
float * positionPointer
Definition Derivs_Limiter.h:29
float getDecelLimit()
get deceleration limit setting
Definition Derivs_Limiter.h:272
float getMaxStoppingDecel()
get setting for how many times accelLimit can be used to stop in time for target position
Definition Derivs_Limiter.h:319
float posLimitHigh
Definition Derivs_Limiter.h:22
bool setVelocity(float vel=0)
set velocity
Definition Derivs_Limiter.h:170
float getTimeInterval()
returns the time (in seconds) between the two most recent calculation times
Definition Derivs_Limiter.h:451
void setPreventGoingTooFast(bool _preventGoingTooFast)
sets value of preventGoingTooFast, true = constrain velocity to velLimit, false decelerate at accelLi...
Definition Derivs_Limiter.h:550
void setTargetAndPosition(float targ=0, float pos=0)
set target and position
Definition Derivs_Limiter.h:138
bool isPosModeNotVelocity()
true if in position target mode, false if in velocity target mode
Definition Derivs_Limiter.h:625
float decelLimit
Definition Derivs_Limiter.h:17
float posLimitLow
Definition Derivs_Limiter.h:21
float getPosition()
get the current position value, but doesn't calculate anything
Definition Derivs_Limiter.h:300
unsigned long lastTime
Definition Derivs_Limiter.h:13
float getAcceleration()
get the current acceleration
Definition Derivs_Limiter.h:291
void setAccelAndDecelLimits(float _accelLimit, float _decelLimit=NAN)
combines setAccelLimit() with setDecelLimit()
Definition Derivs_Limiter.h:230
float getLowPosLimit()
get the lower boundary for position
Definition Derivs_Limiter.h:328