new pattern insist

CHANGELOG.md

@@ -3,11 +3,17 @@
 - Renamed `#define DEBUG_VERBOSE` to `#define DEBUG_TALKATIVE` to make StrokeEngine play nice with WifiManager.
 - Removed state `ERROR` from state machine. After tests with the servo this has become pointless. `disable()` and homing will clear any error state. 
 - Fixed bug with missing implementation of inverted direction signal: https://github.com/theelims/StrokeEngine/issues/3
+- Added an improved pause handling mechanism to pattern:
+  - removed `void patternDelay(int milliseconds)` function as unfit for the purpose
+  - Pattern-Class has 3 private functions `void _startDelay()`, `void _updateDelay(int delayInMillis)` and `bool _isStillDelayed()` to add an internal delay function based on comparing `millis()`.
+  - A pattern can request StrokeEngine to skip the current motion command by returning `_nextMove.skip = true;`. 
+- Speed and acceleration limits are available inside pattern, now. 
+- New pattern available:
+  - Stop'n'Go
+  - Insist
 
 ## Planned Features
 - New Pattern:
-  - Stop'n'Go
-  - Insist
   - Closing Gap
   - Stroke Nibble
   - Jack Hammer

Pattern.md

@@ -17,6 +17,12 @@ Similar to Teasing or Pounding, but every second stroke is only half the depth.
 ### Deeper
 The insertion depth ramps up gradually with each stroke until it reaches its maximum. It then resets and restarts. Sensations controls how many strokes there are in a ramp.
 
+### Stop'n'Go
+Pauses between a series of strokes. The number of strokes ramps from 1 stroke to 5 strokes and back. Sensation changes the length of the pauses between stroke series.
+
+### Insist
+Sensation reduces the effective stroke length while keeping the stroke speed constant to the full stroke. This creates interesting vibrational pattern at higher sensation values. With positive sensation the strokes will wander towards the front, with negative values towards the back.
+
 ## Contribute a Pattern
 Making your own pattern is not that hard. They can be found in the header only [pattern.h](./src/pattern.h) and easily extended.
 
@@ -68,6 +74,9 @@ For debugging and verifying the math it can be handy to have something on the Se
             Serial.println("TimeOfOutStroke: " + String(_timeOfOutStroke));
 #endif
 ```
+
+It is possible for a pattern to insert pauses between strokes. The main stroking-thread of StrokeEngine will poll a new set of motion commands every few milliseconds once the target position of the last stroke is reached. If a pattern returns the motion parameter `_nextMove.skip = true;` inside the costume implementation of the `nextTarget()`-function no new motion is started. Instead it is polled again later. This allows to compare `millis()` inside a pattern. To make this more convenient the `Pattern` base class implements 3 private functions: `void _startDelay()`, `void _updateDelay(int delayInMillis)` and `bool _isStillDelayed()`. `_startDelay()` will start the delay and `_updateDelay(int delayInMillis)` will set the desired pause in milliseconds. `_updateDelay()` can be updated any time with a new value. If a strokes becomes overdue it is executed immediately. `bool _isStillDelayed()` is just a wrapper for comparing the current time with the scheduled time. Can be used inside the `nextTarget()`-function to indicate whether StrokeEngine should be advised to skip this step be returning `_nextMove.skip = true;`. See the pattern Stop'n'Go for an example on how ti use this mechanism.
+
 Don't forget to add an instance of your new pattern class at the very bottom of the file to the `*patternTable[]`-Array.
 ```cpp
 static Pattern *patternTable[] = { 

src/StrokeEngine.cpp

@@ -143,6 +143,7 @@ bool StrokeEngine::setPattern(int patternIndex) {
         patternTable[_patternIndex]->setTimeOfStroke(_timeOfStroke);
         patternTable[_patternIndex]->setStroke(_stroke);
         patternTable[_patternIndex]->setSensation(_sensation);
+        patternTable[_patternIndex]->setSpeedLimit(_maxStepPerSecond, _maxStepAcceleration);
 
         // Reset index counter
         _index = 0; 
@@ -213,7 +214,7 @@ bool StrokeEngine::startPattern() {
 
         // Stop current move, should one be pending (moveToMax or moveToMin)
         if (servo->isRunning()) {
-            // Stop servo motor as fast as legaly allowed
+            // Stop servo motor as fast as legally allowed
             servo->setAcceleration(_maxStepAcceleration);
             servo->applySpeedAcceleration();
             servo->stopMove();
@@ -227,6 +228,7 @@ bool StrokeEngine::startPattern() {
         patternTable[_patternIndex]->setTimeOfStroke(_timeOfStroke);
         patternTable[_patternIndex]->setStroke(_stroke);
         patternTable[_patternIndex]->setSensation(_sensation);
+        patternTable[_patternIndex]->setSpeedLimit(_maxStepPerSecond, _maxStepAcceleration);
 #ifdef DEBUG_TALKATIVE
         Serial.print(" _timeOfStroke: " + String(_timeOfStroke));
         Serial.print(" | _depth: " + String(_depth));
@@ -241,7 +243,7 @@ bool StrokeEngine::startPattern() {
                 "Stroking",             // Name of the task (for debugging)
                 2048,                   // Stack size (bytes)
                 this,                   // Pass reference to this class instance
-                24,                     // Pretty high task piority
+                24,                     // Pretty high task priority
                 &_taskStrokingHandle    // Task handle
             ); 
         } else {
@@ -498,6 +500,8 @@ String StrokeEngine::getPatternName(int index) {
 
 void StrokeEngine::setMaxSpeed(float maxSpeed){
     _maxStepPerSecond = int(0.5 + _motor->maxSpeed * _motor->stepsPerMillimeter);
+    patternTable[_patternIndex]->setSpeedLimit(_maxStepPerSecond, _maxStepAcceleration);
+
 }
 
 float StrokeEngine::getMaxSpeed() {
@@ -506,6 +510,8 @@ float StrokeEngine::getMaxSpeed() {
 
 void StrokeEngine::setMaxAcceleration(float maxAcceleration) {
     _maxStepAcceleration = int(0.5 + _motor->maxAcceleration * _motor->stepsPerMillimeter);
+    patternTable[_patternIndex]->setSpeedLimit(_maxStepPerSecond, _maxStepAcceleration);
+
 }
 
 float StrokeEngine::getMaxAcceleration() {
@@ -517,7 +523,7 @@ void StrokeEngine::_homingProcedure() {
     servo->setSpeedInHz(_homeingSpeed);       
     servo->setAcceleration(_maxStepAcceleration / 10);    
 
-    // Check if we are aleady at the homing switch
+    // Check if we are already at the homing switch
     if (digitalRead(_homeingPin) == !_homeingActiveLow) {
         //back off 5 mm from switch
         servo->move(_motor->stepsPerMillimeter * 2 * _physics->keepoutBoundary * _homeingToBack);

src/pattern.h

@@ -93,6 +93,13 @@ class Pattern {
             return _nextMove;
         } 
 
+        //! Communicates the maximum possible speed and acceleration limits of the machine to a pattern.
+        /*! 
+          @param maxSpeed maximum speed which is possible. Higher speeds get truncated inside StrokeEngine anyway.
+          @param maxAcceleration maximum possible acceleration. Get also truncated, if impossible.
+        */
+        void setSpeedLimit(unsigned int maxSpeed, unsigned int maxAcceleration) { _maxSpeed = maxSpeed; _maxAcceleration = maxAcceleration; } 
+
     protected:
         int _stroke;
         float _timeOfStroke;
@@ -102,6 +109,8 @@ class Pattern {
         motionParameter _nextMove = {0, 0, 0, false};
         int _startDelayMillis = 0;
         int _delayInMillis = 0;
+        unsigned int _maxSpeed = 0;
+        unsigned int _maxAcceleration = 0;
 
         /*!
           @brief Start a delay timer which can be polled by calling _isStillDelayed(). 
@@ -441,14 +450,14 @@ class Deeper : public Pattern {
 
 /**************************************************************************/
 /*!
-  @brief  Closing Gap Stroke Pattern. Pauses between a series of strokes. 
-  The number of strokes changes from 1 stroke to 5 strokes and back. Sensation 
+  @brief  Pauses between a series of strokes. 
+  The number of strokes ramps from 1 stroke to 5 strokes and back. Sensation 
   changes the length of the pauses between stroke series.
 */
 /**************************************************************************/
-class ClosingGap : public Pattern {
+class StopNGo : public Pattern {
     public:
-        ClosingGap(const char *str) : Pattern(str) {}
+        StopNGo(const char *str) : Pattern(str) {}
 
         void setTimeOfStroke(float speed = 0) { 
              // In & Out have same time, so we need to divide by 2
@@ -526,7 +535,90 @@ class ClosingGap : public Pattern {
 
 };
 
+/**************************************************************************/
+/*!
+  @brief  Sensation reduces the effective stroke length while keeping the
+  stroke speed constant to the full stroke. This creates interesting 
+  vibrational pattern at higher sensation values. With positive sensation the
+  strokes will wander towards the front, with negative values towards the back.
+*/
+/**************************************************************************/
+class Insist : public Pattern {
+    public:
+        Insist(const char *str) : Pattern(str) {}   
+
+        void setSensation(float sensation) { 
+            _sensation = sensation;
+
+            // make invert sensation and make into a fraction of the stroke distance
+            _strokeFraction = (100 - abs(sensation))/100.0f;
+
+            _strokeInFront = (sensation > 0) ? true : false;
+
+            _updateStrokeTiming();
+        }
+
+        void setTimeOfStroke(float speed = 0) { 
+             // In & Out have same time, so we need to divide by 2
+            _timeOfStroke = 0.5 * speed;
+            _updateStrokeTiming();
+        }   
 
+        void setStroke(int stroke) {
+            _stroke = stroke;
+            _updateStrokeTiming();
+        }
+
+        motionParameter nextTarget(unsigned int index) {
+
+            // acceleration & speed to meet the profile
+            _nextMove.acceleration = _acceleration;
+            _nextMove.speed = _speed;
+
+            if (_strokeInFront) {
+                // odd stroke is moving out
+                if (index % 2) {
+                    _nextMove.stroke = _stroke - _realStroke;
+
+                // even stroke is moving in
+                } else {
+                    _nextMove.stroke = _stroke;  
+                }
+
+            } else {
+                // odd stroke is moving out    
+                if (index % 2) {
+                    _nextMove.stroke = 0;
+
+                // even stroke is moving in
+                } else {
+                    _nextMove.stroke = 0 + _realStroke;                
+                }
+            }
+
+            _index = index;
+
+            return _nextMove;
+        }
+
+    protected:
+        int _speed = 0;
+        int _acceleration = 0;
+        int _realStroke = 0;
+        float _strokeFraction = 1.0;
+        bool _strokeInFront = false;
+        void _updateStrokeTiming() {
+            // maximum speed of the longest trapezoidal motion (full stroke)
+            _speed = int(1.5 * _stroke/_timeOfStroke);
+
+            // Acceleration to hold 1/3 profile with fractional strokes
+            _acceleration = int(3.0 * _nextMove.speed/(_timeOfStroke * _strokeFraction));
+
+            // Calculate fractional stroke length
+            _realStroke = int((float)_stroke * _strokeFraction);
+        }
+
+};
 
 /**************************************************************************/
 /*
@@ -539,7 +631,8 @@ static Pattern *patternTable[] = {
   new RoboStroke("Robo Stroke"),
   new HalfnHalf("Half'n'Half"),
   new Deeper("Deeper"),
-  new ClosingGap("Closing Gap")
+  new StopNGo("Stop'n'Go"),
+  new Insist("Insist")
   // <-- insert your new pattern class here!
  };