From c956f87562754b82452f79f1ab3a5260347afbf6 Mon Sep 17 00:00:00 2001 From: elims <46279009+theelims@users.noreply.github.com> Date: Tue, 4 Jan 2022 11:30:15 +0100 Subject: [PATCH] Jack Hammer vibrational pattern --- CHANGELOG.md | 5 +- Pattern.md | 7 +- src/PatternMath.h | 12 +-- src/StrokeEngine.cpp | 11 +-- src/pattern.h | 169 ++++++++++++++++++++++++++++++++++++++++++- 5 files changed, 187 insertions(+), 17 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 109d865..18d6646 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,16 +7,17 @@ - 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. +- Steps per mm, speed and acceleration limits are available inside pattern, now. - New pattern available: - Stop'n'Go - Insist + - Jack Hammer ## Planned Features - New Pattern: - Closing Gap - Stroke Nibble - - Jack Hammer + # Release 0.2.0 - Stroking-task uses suspend and resume instead of deleting and recreation to prevent heap fragmentation. diff --git a/Pattern.md b/Pattern.md index 6d7ee45..b640a7b 100644 --- a/Pattern.md +++ b/Pattern.md @@ -23,6 +23,9 @@ Pauses between a series of strokes. The number of strokes ramps from 1 stroke to ### 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. +### Jack Hammer +Vibrational pattern that works like a jack hammer. Vibrates on the way in and pulls out smoothly in one go. Sensation sets the vibration amplitude. + ## 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. @@ -42,7 +45,7 @@ Reimplement set-functions if you need them to do some math: _timeOfStroke = 0.5 * speed; } ``` -Reimplement the core function `motionParameter nextTarget(unsigned int index)`. This is mandatory, as the StrokeEngine will call this fuction after each stroke to get the next set of `motionParameter`. Each time this function is called the `index` increments by 1. If the pattern is called the very first time this always starts as 0. This can be used to create pattern that vary over time: +Reimplement the core function `motionParameter nextTarget(unsigned int index)`. This is mandatory, as the StrokeEngine will call this function after each stroke to get the next set of `motionParameter`. Each time this function is called the `index` increments by 1. If the pattern is called the very first time this always starts as 0. This can be used to create pattern that vary over time: ```cpp motionParameter nextTarget(unsigned int index) { // maximum speed of the trapezoidal motion @@ -75,7 +78,7 @@ For debugging and verifying the math it can be handy to have something on the Se #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. +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 stroke 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 by returning `_nextMove.skip = true;`. See the pattern Stop'n'Go for an example on how to 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 diff --git a/src/PatternMath.h b/src/PatternMath.h index a7451c1..ca69897 100644 --- a/src/PatternMath.h +++ b/src/PatternMath.h @@ -11,10 +11,10 @@ so that it can be made to favor either the end of the output. (Logarithmic mapping) Source: https://playground.arduino.cc/Main/Fscale/ @param originalMin the minimum value of the original range - this MUST be less than origninalMax + this MUST be less than originalMax @param originalMax the maximum value of the original range - this MUST be greater than orginalMin - @param newBegin the end of the new range which maps to orginalMin + this MUST be greater than originalMin + @param newBegin the end of the new range which maps to originalMin it can be smaller, or larger, than newEnd, to facilitate inverting the ranges @param newEnd the end of the new range which maps to originalMax @@ -46,8 +46,8 @@ newEnd, float inputValue, float curve){ if (curve > 10) curve = 10; if (curve < -10) curve = -10; - curve = (curve * -.1) ; // - invert and scale - this seems more intuitive - postive numbers give more weight to high end on output - curve = pow(10, curve); // convert linear scale into lograthimic exponent for other pow function + curve = (curve * -.1) ; // - invert and scale - this seems more intuitive - positive numbers give more weight to high end on output + curve = pow(10, curve); // convert linear scale into logarithmic exponent for other pow function // Check for out of range inputValues if (inputValue < originalMin) { @@ -57,7 +57,7 @@ newEnd, float inputValue, float curve){ inputValue = originalMax; } - // Zero Refference the values + // Zero Reference the values OriginalRange = originalMax - originalMin; if (newEnd > newBegin){ diff --git a/src/StrokeEngine.cpp b/src/StrokeEngine.cpp index 97f66c7..58d2508 100644 --- a/src/StrokeEngine.cpp +++ b/src/StrokeEngine.cpp @@ -140,10 +140,10 @@ bool StrokeEngine::setPattern(int patternIndex) { _patternIndex = patternIndex; // Inject current motion parameters into new pattern + patternTable[_patternIndex]->setSpeedLimit(_maxStepPerSecond, _maxStepAcceleration, _motor->stepsPerMillimeter); patternTable[_patternIndex]->setTimeOfStroke(_timeOfStroke); patternTable[_patternIndex]->setStroke(_stroke); patternTable[_patternIndex]->setSensation(_sensation); - patternTable[_patternIndex]->setSpeedLimit(_maxStepPerSecond, _maxStepAcceleration); // Reset index counter _index = 0; @@ -225,10 +225,11 @@ bool StrokeEngine::startPattern() { // Reset Stroke and Motion parameters _index = -1; + patternTable[_patternIndex]->setSpeedLimit(_maxStepPerSecond, _maxStepAcceleration, _motor->stepsPerMillimeter); 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)); @@ -271,7 +272,7 @@ bool StrokeEngine::startPattern() { void StrokeEngine::stopMotion() { // only valid when if (_state == PATTERN || _state == SETUPDEPTH) { - // Stop servo motor as fast as legaly allowed + // Stop servo motor as fast as legally allowed servo->setAcceleration(_maxStepAcceleration); servo->applySpeedAcceleration(); servo->stopMove(); @@ -500,7 +501,7 @@ String StrokeEngine::getPatternName(int index) { void StrokeEngine::setMaxSpeed(float maxSpeed){ _maxStepPerSecond = int(0.5 + _motor->maxSpeed * _motor->stepsPerMillimeter); - patternTable[_patternIndex]->setSpeedLimit(_maxStepPerSecond, _maxStepAcceleration); + patternTable[_patternIndex]->setSpeedLimit(_maxStepPerSecond, _maxStepAcceleration, _motor->stepsPerMillimeter); } @@ -510,7 +511,7 @@ float StrokeEngine::getMaxSpeed() { void StrokeEngine::setMaxAcceleration(float maxAcceleration) { _maxStepAcceleration = int(0.5 + _motor->maxAcceleration * _motor->stepsPerMillimeter); - patternTable[_patternIndex]->setSpeedLimit(_maxStepPerSecond, _maxStepAcceleration); + patternTable[_patternIndex]->setSpeedLimit(_maxStepPerSecond, _maxStepAcceleration, _motor->stepsPerMillimeter); } diff --git a/src/pattern.h b/src/pattern.h index 9875a11..ffb5df1 100644 --- a/src/pattern.h +++ b/src/pattern.h @@ -97,8 +97,9 @@ class 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. + @param stepsPerMM */ - void setSpeedLimit(unsigned int maxSpeed, unsigned int maxAcceleration) { _maxSpeed = maxSpeed; _maxAcceleration = maxAcceleration; } + void setSpeedLimit(unsigned int maxSpeed, unsigned int maxAcceleration, unsigned int stepsPerMM) { _maxSpeed = maxSpeed; _maxAcceleration = maxAcceleration; _stepsPerMM = stepsPerMM; } protected: int _stroke; @@ -111,6 +112,7 @@ class Pattern { int _delayInMillis = 0; unsigned int _maxSpeed = 0; unsigned int _maxAcceleration = 0; + unsigned int _stepsPerMM = 0; /*! @brief Start a delay timer which can be polled by calling _isStillDelayed(). @@ -620,6 +622,168 @@ class Insist : public Pattern { }; +/**************************************************************************/ +/*! + @brief Vibrational pattern that works like a jack hammer. Vibrates on the + way in and pulls out smoothly in one go. Sensation sets the vibration + amplitude. +*/ +/**************************************************************************/ +class JackHammer : public Pattern { + public: + JackHammer(const char *str) : Pattern(str) {} + void setSensation(float sensation) { + _sensation = sensation; + _updateVibrationParameters(); + } + void setTimeOfStroke(float speed = 0) { + _timeOfStroke = 0.5 * speed; + _strokeInSpeed = int(0.5 * _stroke/_timeOfStroke); + _updateVibrationParameters(); + } + motionParameter nextTarget(unsigned int index) { + + if (_returnStroke == true) { + + // Return strokes goes at regular speed without vibration back to 0 + + // maximum speed of the trapezoidal motion + _nextMove.speed = int(1.5 * _stroke/_timeOfStroke); + // acceleration to meet the profile + _nextMove.acceleration = int(3.0 * float(_nextMove.speed)/_timeOfStroke); + // all they way out to start + _nextMove.stroke = 0; + // clear return flag + _returnStroke = false; + // we are done + return _nextMove; + + } else { + + // Vibration happens at maximum speed and acceleration of the machine + _nextMove.speed = _maxSpeed; + _nextMove.acceleration = _maxAcceleration; + + // odd stroke is shaking out + if (index % 2) { + _nextMove.stroke = _nextMove.stroke - _outVibrationDistance; + // even stroke is shaking in + } else { + _nextMove.stroke = _nextMove.stroke + _inVibrationDistance; + + // check if we have reached the depth target of the stroke and trigger return move + if (_nextMove.stroke >= _stroke) { + _returnStroke = true; + } + } + } + _index = index; + return _nextMove; + } + protected: + bool _returnStroke = false; + int _inVibrationDistance = 0; + int _outVibrationDistance = 0; + int _strokeInSpeed = 0; + void _updateVibrationParameters() { + // Scale vibration amplitude from 1mm to 15mm with sensation + _inVibrationDistance = (int)fscale(-100.0, 100.0, (float)(3.0*_stepsPerMM), (float)(25.0*_stepsPerMM), _sensation, 0.0); + + /* Calculate _outVibrationDistance to match with stroking speed + d_out = d_in * (v_vib - v_stroke) / (v_vib + v_stroke) + Formula neglects acceleration. Real timing will be slower due to finite acceleration & deceleration + */ + _outVibrationDistance = _inVibrationDistance * (_maxSpeed - _strokeInSpeed) / (_maxSpeed + _strokeInSpeed); + +#ifdef DEBUG_PATTERN + Serial.println("_maxSpeed: " + String(_maxSpeed) + " _strokeInSpeed: " + String(_strokeInSpeed) + " _strokeOutSpeed: " + String(int(1.5 * _stroke/_timeOfStroke))); + Serial.println("inDist: " + String(_inVibrationDistance) + " outDist: " + String(_outVibrationDistance)); +#endif + + } +}; + +/**************************************************************************/ +/*! + @brief Vibrational pattern that works like a jack hammer. Vibrates on the + way in and pulls out smoothly in one go. Sensation sets the vibration + amplitude. +*/ +/**************************************************************************/ +class JackHammer : public Pattern { + public: + JackHammer(const char *str) : Pattern(str) {} + void setSensation(float sensation) { + _sensation = sensation; + _updateVibrationParameters(); + } + void setTimeOfStroke(float speed = 0) { + _timeOfStroke = 0.5 * speed; + _strokeInSpeed = int(0.5 * _stroke/_timeOfStroke); + _updateVibrationParameters(); + } + motionParameter nextTarget(unsigned int index) { + + if (_returnStroke == true) { + + // Return strokes goes at regular speed without vibration back to 0 + + // maximum speed of the trapezoidal motion + _nextMove.speed = int(1.5 * _stroke/_timeOfStroke); + // acceleration to meet the profile + _nextMove.acceleration = int(3.0 * float(_nextMove.speed)/_timeOfStroke); + // all they way out to start + _nextMove.stroke = 0; + // clear return flag + _returnStroke = false; + // we are done + return _nextMove; + + } else { + + // Vibration happens at maximum speed and acceleration of the machine + _nextMove.speed = _maxSpeed; + _nextMove.acceleration = _maxAcceleration; + + // odd stroke is shaking out + if (index % 2) { + _nextMove.stroke = _nextMove.stroke - _outVibrationDistance; + // even stroke is shaking in + } else { + _nextMove.stroke = _nextMove.stroke + _inVibrationDistance; + + // check if we have reached the depth target of the stroke and trigger return move + if (_nextMove.stroke >= _stroke) { + _returnStroke = true; + } + } + } + _index = index; + return _nextMove; + } + protected: + bool _returnStroke = false; + int _inVibrationDistance = 0; + int _outVibrationDistance = 0; + int _strokeInSpeed = 0; + void _updateVibrationParameters() { + // Scale vibration amplitude from 3mm to 25mm with sensation + _inVibrationDistance = (int)fscale(-100.0, 100.0, (float)(3.0*_stepsPerMM), (float)(25.0*_stepsPerMM), _sensation, 0.0); + + /* Calculate _outVibrationDistance to match with stroking speed + d_out = d_in * (v_vib - v_stroke) / (v_vib + v_stroke) + Formula neglects acceleration. Real timing will be slower due to finite acceleration & deceleration + */ + _outVibrationDistance = _inVibrationDistance * (_maxSpeed - _strokeInSpeed) / (_maxSpeed + _strokeInSpeed); + +#ifdef DEBUG_PATTERN + Serial.println("_maxSpeed: " + String(_maxSpeed) + " _strokeInSpeed: " + String(_strokeInSpeed) + " _strokeOutSpeed: " + String(int(1.5 * _stroke/_timeOfStroke))); + Serial.println("inDist: " + String(_inVibrationDistance) + " outDist: " + String(_outVibrationDistance)); +#endif + + } +}; + /**************************************************************************/ /* Array holding all different patterns. Please include any custom pattern here. @@ -632,7 +796,8 @@ static Pattern *patternTable[] = { new HalfnHalf("Half'n'Half"), new Deeper("Deeper"), new StopNGo("Stop'n'Go"), - new Insist("Insist") + new Insist("Insist"), + new JackHammer("Jack Hammer") // <-- insert your new pattern class here! };