From 7785e5c5012aa1a605bbeeca980fd9071afe5df4 Mon Sep 17 00:00:00 2001 From: AJ Date: Sun, 26 May 2024 20:56:44 -0600 Subject: [PATCH] Updated Stroke Engine to create the pattern on the fly --- .../lib/StrokeEngine/src/StrokeEngine.cpp | 279 +++---- Software/lib/StrokeEngine/src/StrokeEngine.h | 740 +++++++++--------- Software/lib/StrokeEngine/src/pattern.h | 20 - Software/platformio.ini | 1 - Software/src/constants/copy/en-us.h | 11 +- Software/src/constants/copy/fr.h | 12 +- Software/src/main.cpp | 17 +- Software/src/ossm/OSSM.PatternControls.cpp | 14 +- Software/src/ossm/OSSM.SimplePenetration.cpp | 2 +- Software/src/ossm/OSSM.StrokeEngine.cpp | 40 +- Software/src/ossm/OSSM.cpp | 17 +- Software/src/ossm/OSSM.h | 16 +- Software/src/services/board.h | 8 +- Software/src/services/display.h | 6 + Software/src/services/stepper.h | 47 +- Software/src/services/tasks.h | 1 - Software/src/structs/LanguageStruct.h | 1 + Software/src/structs/SettingPercents.h | 12 +- Software/src/utils/StateLogger.h | 3 - Software/src/utils/StrokeEngineHelper.h | 7 +- Software/src/utils/update.h | 2 + 21 files changed, 635 insertions(+), 621 deletions(-) diff --git a/Software/lib/StrokeEngine/src/StrokeEngine.cpp b/Software/lib/StrokeEngine/src/StrokeEngine.cpp index 1c995f43..b1a8582d 100644 --- a/Software/lib/StrokeEngine/src/StrokeEngine.cpp +++ b/Software/lib/StrokeEngine/src/StrokeEngine.cpp @@ -2,18 +2,12 @@ #include -#include "FastAccelStepper.h" #include "pattern.h" -// static pointer to engine and servo -static FastAccelStepperEngine *engine; -static FastAccelStepper *servo; - +// static pointer to engine and _servo void StrokeEngine::begin(machineGeometry *physics, motorProperties *motor, - FastAccelStepperEngine _engine, - FastAccelStepper *_servo) { - engine = &_engine; - servo = _servo; + FastAccelStepper *servo) { + _servo = servo; // store the machine geometry and motor properties pointer _physics = physics; _motor = motor; @@ -30,7 +24,6 @@ void StrokeEngine::begin(machineGeometry *physics, motorProperties *motor, // Initialize with default values _state = UNDEFINED; _isHomed = false; - _patternIndex = 0; _index = 0; _depth = _maxStep; _previousDepth = _maxStep; @@ -39,16 +32,13 @@ void StrokeEngine::begin(machineGeometry *physics, motorProperties *motor, _timeOfStroke = 1.0; _sensation = 0.0; - // Setup FastAccelStepper -// engine.init(); -// servo = engine.stepperConnectToPin(_motor->stepPin); - if (servo) { - servo->setDirectionPin(_motor->directionPin, _motor->invertDirection); - servo->setEnablePin(_motor->enablePin, _motor->enableActiveLow); - servo->setAutoEnable(false); - servo->disableOutputs(); + if (_servo) { + _servo->setDirectionPin(_motor->directionPin, _motor->invertDirection); + _servo->setEnablePin(_motor->enablePin, _motor->enableActiveLow); + _servo->setAutoEnable(false); + _servo->disableOutputs(); } - Serial.println("Servo initialized"); + Serial.println("_servo initialized"); #ifdef DEBUG_TALKATIVE Serial.println("Stroke Engine State: " + verboseState[_state]); @@ -63,7 +53,7 @@ void StrokeEngine::setSpeed(float speed, bool applyNow = false) { // Constrain stroke time between 10ms and 120 seconds _timeOfStroke = constrain(60.0 / speed, 0.01, 120.0); - patternTable[_patternIndex]->setTimeOfStroke(_timeOfStroke); + pattern->setTimeOfStroke(_timeOfStroke); #ifdef DEBUG_TALKATIVE Serial.println("setTimeOfStroke: " + String(_timeOfStroke, 2)); @@ -96,7 +86,7 @@ void StrokeEngine::setDepth(float depth, bool applyNow = false) { _depth = constrain(int(depth * _motor->stepsPerMillimeter), _minStep, _maxStep); - patternTable[_patternIndex]->setDepth(_depth); + pattern->setDepth(_depth); #ifdef DEBUG_TALKATIVE Serial.println("setDepth: " + String(_depth)); @@ -135,7 +125,7 @@ void StrokeEngine::setStroke(float stroke, bool applyNow = false) { _stroke = constrain(int(stroke * _motor->stepsPerMillimeter), _minStep, _maxStep); - patternTable[_patternIndex]->setStroke(_stroke); + pattern->setStroke(_stroke); #ifdef DEBUG_TALKATIVE Serial.println("setStroke: " + String(_stroke)); @@ -173,7 +163,7 @@ void StrokeEngine::setSensation(float sensation, bool applyNow = false) { // Constrain sensation between -100 and 100 _sensation = constrain(sensation, -100, 100); - patternTable[_patternIndex]->setSensation(_sensation); + pattern->setSensation(_sensation); #ifdef DEBUG_TALKATIVE Serial.println("setSensation: " + String(_sensation)); @@ -201,68 +191,60 @@ void StrokeEngine::setSensation(float sensation, bool applyNow = false) { float StrokeEngine::getSensation() { return _sensation; } -bool StrokeEngine::setPattern(int patternIndex, bool applyNow = false) { - // Check wether pattern Index is in range - if ((patternIndex < patternTableSize) && (patternIndex >= 0) && - (patternIndex != _patternIndex)) { - _patternIndex = patternIndex; +bool StrokeEngine::setPattern(Pattern *NextPattern, + bool applyNow = false) { + // Free up memory from previous pattern - // Inject current motion parameters into new pattern - if (xSemaphoreTake(_patternMutex, portMAX_DELAY) == pdTRUE) { - patternTable[_patternIndex]->setSpeedLimit( - _maxStepPerSecond, _maxStepAcceleration, - _motor->stepsPerMillimeter); - patternTable[_patternIndex]->setTimeOfStroke(_timeOfStroke); - patternTable[_patternIndex]->setStroke(_stroke); - patternTable[_patternIndex]->setDepth(_depth); - patternTable[_patternIndex]->setSensation(_sensation); - - // When running a pattern and immediate update requested: - if ((_state == PATTERN) && (applyNow == true)) { - // set flag to apply update from stroking thread - _applyUpdate = true; + delete pattern; + pattern = NextPattern; -#ifdef DEBUG_TALKATIVE - Serial.println("Apply New Settings Now"); -#endif - } - - // Reset index counter - _index = -1; + // Inject current motion parameters into new pattern + if (xSemaphoreTake(_patternMutex, portMAX_DELAY) == pdTRUE) { + pattern->setSpeedLimit(_maxStepPerSecond, _maxStepAcceleration, + _motor->stepsPerMillimeter); + pattern->setTimeOfStroke(_timeOfStroke); + pattern->setStroke(_stroke); + pattern->setDepth(_depth); + pattern->setSensation(_sensation); - // give back mutex - xSemaphoreGive(_patternMutex); - } + // When running a pattern and immediate update requested: + if ((_state == PATTERN) && (applyNow == true)) { + // set flag to apply update from stroking thread + _applyUpdate = true; #ifdef DEBUG_TALKATIVE - Serial.println("setPattern: [" + String(_patternIndex) + "] " + - patternTable[_patternIndex]->getName()); - Serial.println("setTimeOfStroke: " + String(_timeOfStroke, 2)); - Serial.println("setDepth: " + String(_depth)); - Serial.println("setStroke: " + String(_stroke)); - Serial.println("setSensation: " + String(_sensation)); + Serial.println("Apply New Settings Now"); #endif - return true; + } + + // Reset index counter + _index = -1; + + // give back mutex + xSemaphoreGive(_patternMutex); } - // Return false on no match #ifdef DEBUG_TALKATIVE - Serial.println("Failed to set pattern: " + String(_patternIndex)); + Serial.println("setPattern: " + String(pattern->getName())); + Serial.println("setTimeOfStroke: " + String(_timeOfStroke, 2)); + Serial.println("setDepth: " + String(_depth)); + Serial.println("setStroke: " + String(_stroke)); + Serial.println("setSensation: " + String(_sensation)); #endif - return false; + return true; } -int StrokeEngine::getPattern() { return _patternIndex; } +int StrokeEngine::getPattern() { return 0; } bool StrokeEngine::startPattern() { // Only valid if state is ready if (_state == READY || _state == SETUPDEPTH) { // Stop current move, should one be pending (moveToMax or moveToMin) - if (servo->isRunning()) { - // Stop servo motor as fast as legally allowed - servo->setAcceleration(_maxStepAcceleration); - servo->applySpeedAcceleration(); - servo->stopMove(); + if (_servo->isRunning()) { + // Stop _servo motor as fast as legally allowed + _servo->setAcceleration(_maxStepAcceleration); + _servo->applySpeedAcceleration(); + _servo->stopMove(); } // Set state to PATTERN @@ -271,13 +253,12 @@ bool StrokeEngine::startPattern() { // Reset Stroke and Motion parameters _index = -1; if (xSemaphoreTake(_patternMutex, portMAX_DELAY) == pdTRUE) { - patternTable[_patternIndex]->setSpeedLimit( - _maxStepPerSecond, _maxStepAcceleration, - _motor->stepsPerMillimeter); - patternTable[_patternIndex]->setTimeOfStroke(_timeOfStroke); - patternTable[_patternIndex]->setStroke(_stroke); - patternTable[_patternIndex]->setDepth(_depth); - patternTable[_patternIndex]->setSensation(_sensation); + pattern->setSpeedLimit(_maxStepPerSecond, _maxStepAcceleration, + _motor->stepsPerMillimeter); + pattern->setTimeOfStroke(_timeOfStroke); + pattern->setStroke(_stroke); + pattern->setDepth(_depth); + pattern->setSensation(_sensation); xSemaphoreGive(_patternMutex); } @@ -325,24 +306,24 @@ void StrokeEngine::stopMotion() { // Set state _state = READY; - // Stop servo motor as fast as legally allowed - servo->setAcceleration(_maxStepAcceleration); - servo->applySpeedAcceleration(); - servo->stopMove(); + // Stop _servo motor as fast as legally allowed + _servo->setAcceleration(_maxStepAcceleration); + _servo->applySpeedAcceleration(); + _servo->stopMove(); #ifdef DEBUG_TALKATIVE Serial.println("Motion stopped"); #endif - // Wait for servo stopped - while (servo->isRunning()) + // Wait for _servo stopped + while (_servo->isRunning()) ; // Send telemetry data if (_callbackTelemetry != NULL) { - _callbackTelemetry( - float(servo->getCurrentPosition() / _motor->stepsPerMillimeter), - 0.0, false); + _callbackTelemetry(float(_servo->getCurrentPosition() / + _motor->stepsPerMillimeter), + 0.0, false); } } @@ -377,8 +358,8 @@ void StrokeEngine::enableAndHome(endstopProperties *endstop, float speed) { // first stop current motion and delete stroke task stopMotion(); - // Enable Servo - servo->enableOutputs(); + // Enable _servo + _servo->enableOutputs(); // Create homing task xTaskCreatePinnedToCore( @@ -400,19 +381,19 @@ void StrokeEngine::thisIsHome(float speed) { _homeingSpeed = speed * _motor->stepsPerMillimeter; if (_state == UNDEFINED) { - // Enable Servo - servo->enableOutputs(); + // Enable _servo + _servo->enableOutputs(); // Stet current position as home - servo->setCurrentPosition(-_motor->stepsPerMillimeter * - _physics->keepoutBoundary); + _servo->setCurrentPosition(-_motor->stepsPerMillimeter * + _physics->keepoutBoundary); // Set feedrate for homing - servo->setSpeedInHz(_homeingSpeed); - servo->setAcceleration(_maxStepAcceleration / 10); + _servo->setSpeedInHz(_homeingSpeed); + _servo->setAcceleration(_maxStepAcceleration / 10); // drive free of switch and set axis to 0 - servo->moveTo(_minStep); + _servo->moveTo(_minStep); // Change state _isHomed = true; @@ -441,10 +422,10 @@ bool StrokeEngine::moveToMax(float speed) { // Set feedrate for safe move // Constrain speed between 1 step/sec and _maxStepPerSecond - servo->setSpeedInHz(constrain(speed * _motor->stepsPerMillimeter, 1, - _maxStepPerSecond)); - servo->setAcceleration(_maxStepAcceleration / 10); - servo->moveTo(_maxStep); + _servo->setSpeedInHz(constrain(speed * _motor->stepsPerMillimeter, 1, + _maxStepPerSecond)); + _servo->setAcceleration(_maxStepAcceleration / 10); + _servo->moveTo(_maxStep); // Send telemetry data if (_callbackTelemetry != NULL) { @@ -476,10 +457,10 @@ bool StrokeEngine::moveToMin(float speed) { // Set feedrate for safe move // Constrain speed between 1 step/sec and _maxStepPerSecond - servo->setSpeedInHz(constrain(speed * _motor->stepsPerMillimeter, 1, - _maxStepPerSecond)); - servo->setAcceleration(_maxStepAcceleration / 10); - servo->moveTo(_minStep); + _servo->setSpeedInHz(constrain(speed * _motor->stepsPerMillimeter, 1, + _maxStepPerSecond)); + _servo->setAcceleration(_maxStepAcceleration / 10); + _servo->moveTo(_minStep); // Send telemetry data if (_callbackTelemetry != NULL) { @@ -517,9 +498,9 @@ bool StrokeEngine::setupDepth(float speed, bool fancy) { // Set feedrate for safe move // Constrain speed between 1 step/sec and _maxStepPerSecond - servo->setSpeedInHz(constrain(speed * _motor->stepsPerMillimeter, 1, - _maxStepPerSecond)); - servo->setAcceleration(_maxStepAcceleration / 10); + _servo->setSpeedInHz(constrain(speed * _motor->stepsPerMillimeter, 1, + _maxStepPerSecond)); + _servo->setAcceleration(_maxStepAcceleration / 10); // Set new state _state = SETUPDEPTH; @@ -542,8 +523,8 @@ void StrokeEngine::disable() { _state = UNDEFINED; _isHomed = false; - // Disable servo motor - servo->disableOutputs(); + // Disable _servo motor + _servo->disableOutputs(); // Delete homing Task if (_taskHomingHandle != NULL) { @@ -552,17 +533,13 @@ void StrokeEngine::disable() { } #ifdef DEBUG_TALKATIVE - Serial.println("Servo disabled. Call home to continue."); + Serial.println("_servo disabled. Call home to continue."); Serial.println("Stroke Engine State: " + verboseState[_state]); #endif } String StrokeEngine::getPatternName(int index) { - if (index >= 0 && index <= patternTableSize) { - return String(patternTable[index]->getName()); - } else { - return String("Invalid"); - } + return String("Invalid"); } void StrokeEngine::setMaxSpeed(float maxSpeed) { @@ -571,9 +548,8 @@ void StrokeEngine::setMaxSpeed(float maxSpeed) { // Convert speed into steps _maxStepPerSecond = int(0.5 + _motor->maxSpeed * _motor->stepsPerMillimeter); - patternTable[_patternIndex]->setSpeedLimit(_maxStepPerSecond, - _maxStepAcceleration, - _motor->stepsPerMillimeter); + pattern->setSpeedLimit(_maxStepPerSecond, _maxStepAcceleration, + _motor->stepsPerMillimeter); xSemaphoreGive(_patternMutex); } } @@ -588,9 +564,8 @@ void StrokeEngine::setMaxAcceleration(float maxAcceleration) { // Convert acceleration into steps _maxStepAcceleration = int(0.5 + _motor->maxAcceleration * _motor->stepsPerMillimeter); - patternTable[_patternIndex]->setSpeedLimit(_maxStepPerSecond, - _maxStepAcceleration, - _motor->stepsPerMillimeter); + pattern->setSpeedLimit(_maxStepPerSecond, _maxStepAcceleration, + _motor->stepsPerMillimeter); xSemaphoreGive(_patternMutex); } } @@ -607,56 +582,56 @@ void StrokeEngine::registerTelemetryCallback(void (*callbackTelemetry)(float, void StrokeEngine::_homingProcedure() { // Set feedrate for homing - servo->setSpeedInHz(_homeingSpeed); - servo->setAcceleration(_maxStepAcceleration / 10); + _servo->setSpeedInHz(_homeingSpeed); + _servo->setAcceleration(_maxStepAcceleration / 10); // 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); + _servo->move(_motor->stepsPerMillimeter * 2 * + _physics->keepoutBoundary * _homeingToBack); // wait for move to complete - while (servo->isRunning()) { + while (_servo->isRunning()) { // Pause the task for 100ms while waiting for move to complete vTaskDelay(100 / portTICK_PERIOD_MS); } // move back towards endstop - servo->move(-_motor->stepsPerMillimeter * 4 * - _physics->keepoutBoundary * _homeingToBack); + _servo->move(-_motor->stepsPerMillimeter * 4 * + _physics->keepoutBoundary * _homeingToBack); } else { // Move MAX_TRAVEL towards the homing switch - servo->move(-_motor->stepsPerMillimeter * _physics->physicalTravel * - _homeingToBack); + _servo->move(-_motor->stepsPerMillimeter * _physics->physicalTravel * + _homeingToBack); } // Poll homing switch - while (servo->isRunning()) { + while (_servo->isRunning()) { // Switch is active low if (digitalRead(_homeingPin) == !_homeingActiveLow) { // Set home position if (_homeingToBack == 1) { // Switch is at -KEEPOUT_BOUNDARY - servo->forceStopAndNewPosition(-_motor->stepsPerMillimeter * - _physics->keepoutBoundary); + _servo->forceStopAndNewPosition(-_motor->stepsPerMillimeter * + _physics->keepoutBoundary); // drive free of switch and set axis to lower end - servo->moveTo(_minStep); + _servo->moveTo(_minStep); } else { - servo->forceStopAndNewPosition( + _servo->forceStopAndNewPosition( _motor->stepsPerMillimeter * (_physics->physicalTravel - _physics->keepoutBoundary)); // drive free of switch and set axis to front end - servo->moveTo(_maxStep); + _servo->moveTo(_maxStep); } _isHomed = true; // drive free of switch and set axis to 0 - servo->moveTo(0); + _servo->moveTo(0); // Break loop, home was found break; @@ -666,9 +641,9 @@ void StrokeEngine::_homingProcedure() { vTaskDelay(20 / portTICK_PERIOD_MS); } - // disable Servo if homing has not found the homing switch + // disable _servo if homing has not found the homing switch if (!_isHomed) { - servo->disableOutputs(); + _servo->disableOutputs(); _state = UNDEFINED; #ifdef DEBUG_TALKATIVE @@ -718,19 +693,19 @@ void StrokeEngine::_stroking() { if (xSemaphoreTake(_patternMutex, 0) == pdTRUE) { if (_applyUpdate == true) { // Ask pattern for update on motion parameters - currentMotion = patternTable[_patternIndex]->nextTarget(_index); + currentMotion = pattern->nextTarget(_index); // Increase deceleration if required to avoid crash - if (servo->getAcceleration() > currentMotion.acceleration) { + if (_servo->getAcceleration() > currentMotion.acceleration) { #ifdef DEBUG_CLIPPING Serial.print("Crash avoidance! Set Acceleration from " + String(currentMotion.acceleration)); - Serial.println(" to " + String(servo->getAcceleration())); + Serial.println(" to " + String(_servo->getAcceleration())); #endif - currentMotion.acceleration = servo->getAcceleration(); + currentMotion.acceleration = _servo->getAcceleration(); } - // Apply new trapezoidal motion profile to servo + // Apply new trapezoidal motion profile to _servo _applyMotionProfile(¤tMotion); // clear update flag @@ -738,19 +713,19 @@ void StrokeEngine::_stroking() { } // If motor has stopped issue moveTo command to next position - else if (servo->isRunning() == false) { + else if (_servo->isRunning() == false) { // Increment index for pattern _index++; // Querey new set of pattern parameters - currentMotion = patternTable[_patternIndex]->nextTarget(_index); + currentMotion = pattern->nextTarget(_index); // Pattern may introduce pauses between strokes if (currentMotion.skip == false) { #ifdef DEBUG_STROKE Serial.println("Stroking Index: " + String(_index)); #endif - // Apply new trapezoidal motion profile to servo + // Apply new trapezoidal motion profile to _servo _applyMotionProfile(¤tMotion); } else { @@ -787,7 +762,7 @@ void StrokeEngine::_applyMotionProfile(motionParameter *motion) { float speed = 0.0; float position = 0.0; - // Apply new trapezoidal motion profile to servo if pattern does not skip + // Apply new trapezoidal motion profile to _servo if pattern does not skip if (motion->skip == false) { // Constrain speed to below _maxStepPerSecond if (motion->speed > _maxStepPerSecond) { @@ -823,10 +798,10 @@ void StrokeEngine::_applyMotionProfile(motionParameter *motion) { // Constrain stroke to motion envelope int pos = constrain((motion->stroke), _minStep, _maxStep); - // write values to servo - servo->setSpeedInHz(motion->speed); - servo->setAcceleration(motion->acceleration); - servo->moveTo(pos); + // write values to _servo + _servo->setSpeedInHz(motion->speed); + _servo->setAcceleration(motion->acceleration); + _servo->moveTo(pos); // Compile speed telemetry data speed = float(motion->speed / _motor->stepsPerMillimeter); @@ -866,13 +841,13 @@ void StrokeEngine::_setupDepths() { #endif } - // move servo to desired position - servo->moveTo(depth); + // move _servo to desired position + _servo->moveTo(depth); // Send telemetry data if (_callbackTelemetry != NULL) { _callbackTelemetry(float(depth / _motor->stepsPerMillimeter), - float(servo->getSpeedInMilliHz() * 1000 / + float(_servo->getSpeedInMilliHz() * 1000 / _motor->stepsPerMillimeter), false); } diff --git a/Software/lib/StrokeEngine/src/StrokeEngine.h b/Software/lib/StrokeEngine/src/StrokeEngine.h index a5c8059a..05fae41a 100644 --- a/Software/lib/StrokeEngine/src/StrokeEngine.h +++ b/Software/lib/StrokeEngine/src/StrokeEngine.h @@ -1,7 +1,7 @@ /** * StrokeEngine - * A library to create a variety of stroking motions with a stepper or servo motor on an ESP32. - * https://github.com/theelims/StrokeEngine + * A library to create a variety of stroking motions with a stepper or servo + * motor on an ESP32. https://github.com/theelims/StrokeEngine * * Copyright (C) 2022 theelims * @@ -17,10 +17,11 @@ #include "pattern.h" // Debug Levels -//#define DEBUG_TALKATIVE // Show debug messages from the StrokeEngine on Serial -//#define DEBUG_STROKE // Show debug messaged for each individual stroke on Serial -#define DEBUG_CLIPPING // Show debug messages when motions violating the machine - // physics are commanded +// #define DEBUG_TALKATIVE // Show debug messages from the +// StrokeEngine on Serial #define DEBUG_STROKE // Show debug +// messaged for each individual stroke on Serial +#define DEBUG_CLIPPING // Show debug messages when motions violating the + // machine physics are commanded /**************************************************************************/ /*! @@ -28,46 +29,49 @@ */ /**************************************************************************/ typedef struct { - float physicalTravel; /*> What is the maximum physical travel in mm */ - float keepoutBoundary; /*> Soft endstop preventing hard crashes in mm. Will be - * subtracted twice from physicalTravel. Should be - * sufficiently to completley drive clear from - * homing switch */ + float physicalTravel; /*> What is the maximum physical travel in mm */ + float keepoutBoundary; /*> Soft endstop preventing hard crashes in mm. Will + * be subtracted twice from physicalTravel. Should be + * sufficiently to completley drive clear from + * homing switch */ } machineGeometry; /**************************************************************************/ /*! - @brief Struct defining the motor (stepper or servo with STEP/DIR - interface) and the motion system translating the rotation into a + @brief Struct defining the motor (stepper or servo with STEP/DIR + interface) and the motion system translating the rotation into a linear motion. */ /**************************************************************************/ typedef struct { - float maxSpeed; /*> What is the maximum speed in mm/s */ - float maxAcceleration; /*> Maximum acceleration in mm/s^2 */ - float stepsPerMillimeter; /*> Number of steps per millimeter */ - bool invertDirection; /*> Set to true to invert the direction signal - * The firmware expects the home switch to be located at the - * end of an retraction move. That way the machine homes - * itself away from the body. Home position is -KEEPOUTBOUNDARY */ - bool enableActiveLow; /*> Polarity of the enable signal. True for active low. */ - int stepPin; /*> Pin connected to the STEP input */ - int directionPin; /*> Pin connected to the DIR input */ - int enablePin; /*> Pin connected to the ENA input */ + float maxSpeed; /*> What is the maximum speed in mm/s */ + float maxAcceleration; /*> Maximum acceleration in mm/s^2 */ + float stepsPerMillimeter; /*> Number of steps per millimeter */ + bool invertDirection; /*> Set to true to invert the direction signal + * The firmware expects the home switch to be located + * at the end of an retraction move. That way the + * machine homes itself away from the body. Home + * position is -KEEPOUTBOUNDARY */ + bool enableActiveLow; /*> Polarity of the enable signal. True for active + low. */ + int stepPin; /*> Pin connected to the STEP input */ + int directionPin; /*> Pin connected to the DIR input */ + int enablePin; /*> Pin connected to the ENA input */ } motorProperties; /**************************************************************************/ /*! - @brief Struct defining the endstop properties like pin, pinmode, polarity + @brief Struct defining the endstop properties like pin, pinmode, polarity and homing direction. */ /**************************************************************************/ typedef struct { - bool homeToBack; /*> Set to true to home to the back of the machine - * Set to false to home to the front of the machine */ - bool activeLow; /*> Polarity of the homing signal. True for active low. */ - int endstopPin; /*> Pin connected to home switch */ - uint8_t pinMode; /*> Pinmode of the switch INPUT, INPUT_PULLUP, INPUT_PULLDOWN */ + bool homeToBack; /*> Set to true to home to the back of the machine + * Set to false to home to the front of the machine */ + bool activeLow; /*> Polarity of the homing signal. True for active low. */ + int endstopPin; /*> Pin connected to home switch */ + uint8_t pinMode; /*> Pinmode of the switch INPUT, INPUT_PULLUP, + INPUT_PULLDOWN */ } endstopProperties; /**************************************************************************/ @@ -76,350 +80,358 @@ typedef struct { */ /**************************************************************************/ typedef enum { - UNDEFINED, //!< No power to the servo. We don't know its position - READY, //!< Servo is energized and knows it position. Not running. - PATTERN, //!< Stroke Engine is running and servo is moving according to defined pattern. - SETUPDEPTH, //!< Interactive adjustment mode to setup depth and stroke - STREAMING //!< Tracks the depth-position whenever depth is updated. + UNDEFINED, //!< No power to the servo. We don't know its position + READY, //!< Servo is energized and knows it position. Not running. + PATTERN, //!< Stroke Engine is running and servo is moving according to + //!< defined pattern. + SETUPDEPTH, //!< Interactive adjustment mode to setup depth and stroke + STREAMING //!< Tracks the depth-position whenever depth is updated. } ServoState; // Verbose strings of states for debugging purposes static String verboseState[] = { - "[0] Servo disabled", - "[1] Servo ready", - "[2] Servo pattern running", - "[3] Servo setup depth", - "[4] Servo position streaming" -}; + "[0] Servo disabled", "[1] Servo ready", "[2] Servo pattern running", + "[3] Servo setup depth", "[4] Servo position streaming"}; /**************************************************************************/ /*! @brief Stroke Engine provides a convenient package for stroking motions - created by stepper or servo motors. It's internal states are handled by a - finite state machine. A pattern generator allows to creat a variety of + created by stepper or servo motors. It's internal states are handled by a + finite state machine. A pattern generator allows to creat a variety of motion profiles. Under the hood FastAccelStepper is used for interfacing - a stepper or servo motor vie a STEP/DIR interface. + a stepper or servo motor vie a STEP/DIR interface. */ /**************************************************************************/ class StrokeEngine { - public: - - /**************************************************************************/ - /*! - @brief Initializes FastAccelStepper and configures all pins and outputs - accordingly. StrokeEngine is in state UNDEFINED - */ - /**************************************************************************/ - void begin(machineGeometry *physics, motorProperties *motor, FastAccelStepperEngine engine, FastAccelStepper *servo); - - /**************************************************************************/ - /*! - @brief Set the speed of a stroke. Speed is given in Strokes per Minute - and internally calculated to the time a full stroke needs to complete. - Settings tale effect with next stroke, or after calling - applyNewSettingsNow(). - @param speed Strokes per Minute. Is constrained from 0.5 to 6000 - @param applyNow Set to true if changes should take effect immediately - */ - /**************************************************************************/ - void setSpeed(float speed, bool applyNow); - - /**************************************************************************/ - /*! - @brief Get the speed of a stroke. Speed is returned as Strokes per Minute. - @return Strokes per Minute. - */ - /**************************************************************************/ - float getSpeed(); - - /**************************************************************************/ - /*! - @brief Set the depth of a stroke. Settings tale effect with next stroke, - or after calling applyNewSettingsNow(). - @param depth Depth in [mm]. Is constrained from 0 to TRAVEL - @param applyNow Set to true if changes should take effect immediately - */ - /**************************************************************************/ - void setDepth(float depth, bool applyNow); - - /**************************************************************************/ - /*! - @brief Returns the depth of a stroke. - @return Depth in [mm]. Is constrained from 0 to TRAVEL - */ - /**************************************************************************/ - float getDepth(); - - /**************************************************************************/ - /*! - @brief Set the stroke length of a stroke. Settings take effect with next - stroke, or after calling applyNewSettingsNow(). - @param stroke Stroke length in [mm]. Is constrained from 0 to TRAVEL - @param applyNow Set to true if changes should take effect immediately - */ - /**************************************************************************/ - void setStroke(float stroke, bool applyNow); - - /**************************************************************************/ - /*! - @brief Get the stroke length of a stroke. - @return Stroke length in [mm]. - */ - /**************************************************************************/ - float getStroke(); - - /**************************************************************************/ - /*! - @brief Set the sensation of a pattern. Sensation is an additional - parameter a pattern may use to alter its behaviour. Settings takes - effect with next stroke, or after calling applyNewSettingsNow(). - @param sensation Sensation in [a.u.]. Is constrained from -100 to 100 - with 0 beeing assumed as neutral. - @param applyNow Set to true if changes should take effect immediately - */ - /**************************************************************************/ - void setSensation(float sensation, bool applyNow); - - /**************************************************************************/ - /*! - @brief Get the sensation of a pattern. Sensation is an additional - parameter a pattern may use to alter its behaviour. - @return Sensation in [a.u.]. Is constrained from -100 to 100 - with 0 beeing assumed as neutral. - */ - /**************************************************************************/ - float getSensation(); - - /**************************************************************************/ - /*! - @brief Choose a pattern for the StrokeEngine. Settings take effect with - next stroke, or after calling applyNewSettingsNow(). - @param patternIndex Index of a pattern - @param applyNow Set to true if changes should take effect immediately - @return TRUE on success, FALSE, if patternIndex is invalid. Previous - pattern will be retained. - */ - /**************************************************************************/ - bool setPattern(int patternIndex, bool applyNow); - - /**************************************************************************/ - /*! - @brief Get the pattern index for the StrokeEngine. - @return Index of a pattern - */ - /**************************************************************************/ - int getPattern(); - - /**************************************************************************/ - /*! - @brief Creates a FreeRTOS task to run a stroking pattern. Only valid in - state READY. Pattern is initialized with the values from the set - functions. If the task is running, state is PATTERN. - @return TRUE when task was created and motion starts, FALSE on failure. - */ - /**************************************************************************/ - bool startPattern(); - - /**************************************************************************/ - /*! - @brief Stops the motion with MAX_ACCEL and deletes the stroking task. Is - in state READY afterwards. - */ - /**************************************************************************/ - void stopMotion(); - - /**************************************************************************/ - /*! - @brief Enable the servo/stepper and do the homing procedure. Drives towards - the endstop with HOMING_SPEED. Function is non-blocking and backed by a task. - Optionally a callback can be given to receive feedback if homing succeeded - going in state READY. If homing switch is not found after traveling - MAX_TRAVEL it times out, disables the servo and goes into UNDEFINED. - @param endstop Pointer to a endstopProperties struct defining all relevant - properties like pin, pinmode, homing direction & signal - polarity. - @param speed Speed in mm/s used for finding the homing switch. - Defaults to 5.0 mm/s - @param callBackHoming Callback function is called after homing is done. - Function parametere holds a bool containing the success (TRUE) - or failure (FALSE) of homing. - */ - /**************************************************************************/ - void enableAndHome(endstopProperties *endstop, float speed = 5.0); - void enableAndHome(endstopProperties *endstop, void(*callBackHoming)(bool), float speed = 5.0); - - /**************************************************************************/ - /*! - @brief If no homing switch is present homing can be done manually. Push - the endeffector all the way in and call thisIsHome(). This enables the - the servo and sets the position to -KEEPOUT_BOUNDARY - @param speed Speed in mm/s used for finding the homing switch. - Defaults to 5.0 mm/s - */ - /**************************************************************************/ - void thisIsHome(float speed = 5.0); - - /**************************************************************************/ - /*! - @brief In state PATTERN, SETUPDEPTH and READY this - moves the endeffector to TRAVEL. Can be used for adjustments. Stops any - running pattern and ends in state READY. - @param speed Speed in mm/s used for driving to max. - Defaults to 10.0 mm/s - @return TRUE on success, FALSE if state does not allow this. - */ - /**************************************************************************/ - bool moveToMax(float speed = 10.0); - - /**************************************************************************/ - /*! - @brief In state PATTERN, SETUPDEPTH and READY this - moves the endeffector to 0. Can be used for adjustments. Stops any running - pattern and ends in state READY. - @param speed Speed in mm/s used for driving to min. - Defaults to 10.0 mm/s - @return TRUE on success, FALSE if state does not allow this. - */ - /**************************************************************************/ - bool moveToMin(float speed = 10.0); - - /**************************************************************************/ - /*! - @brief In state PATTERN and READY this moves the endeffector - to DEPTH and enters state SETUPDEPTH. Follows the DEPTH postion - whenever setDepth() is called. Can be used for adjustments. Stops any running - pattern. - @param speed Speed in mm/s used for driving to min. - Defaults to 10.0 mm/s - @param fancy In fancy mode sensation allows to adjust both, depth and - stroke. +100 adjusts the depth position, -100 adjusts the - stroke position. 0 adjusts the midpoint depth-stroke/2. - @return TRUE on success, FALSE if state does not allow this. - */ - /**************************************************************************/ - bool setupDepth(float speed = 10.0, bool fancy = false); - - /**************************************************************************/ - /*! - @brief Retrieves the current servo state from the internal state machine. - @return Current state of the state machine - */ - /**************************************************************************/ - ServoState getState(); - - /**************************************************************************/ - /*! - @brief Disables the servo motor instantly and deletes any motion task. - Sets state machine to UNDEFINED. Must be followed by homing to enable - servo again. - */ - /**************************************************************************/ - void disable(); - - /**************************************************************************/ - /*! - @brief Makes the pattern list available for the main program to retreive - informations like pattern names. - @param index index of a pattern. - @return String holding a pattern name with a certain index. If index is - out of range it returns "Invalid" - */ - /**************************************************************************/ - String getPatternName(int index); - - /**************************************************************************/ - /*! - @brief Makes the pattern list available for the main program to retreive - informations like pattern names. - @return The number of pattern available. - */ - /**************************************************************************/ - unsigned int getNumberOfPattern() { - return patternTableSize; - }; - - /**************************************************************************/ - /*! - @brief Updates the maximum speed number of StrokeEngine. This value is - used to keep alle motions in check and as a safeguard. - @param maxSpeed maximum Speed in mm/s - */ - /**************************************************************************/ - void setMaxSpeed(float maxSpeed); - - /**************************************************************************/ - /*! - @brief Get the current set maximum speed - @return maximum speed in mm/s - */ - /**************************************************************************/ - float getMaxSpeed(); - - /**************************************************************************/ - /*! - @brief Updates the maximum acceleration number of StrokeEngine. This value - is used to keep alle motions in check and as a safeguard. - @param maxAcceleration maximum acceleration in mm/s² - */ - /**************************************************************************/ - void setMaxAcceleration(float maxAcceleration); - - /**************************************************************************/ - /*! - @brief Get the current set maximum acceleration - @return maximum acceleration in mm/s² - */ - /**************************************************************************/ - float getMaxAcceleration(); - - /**************************************************************************/ - /*! - @brief Register a callback function that will update telemetry information - about StrokeEngine. The provided function will be called whenever a motion - is executed by a manual command or by a pattern. The returned values are the - target position of this move, its top speed and wether clipping occurred. - @param callbackTelemetry Function must be of type: - void callbackTelemetry(float position, float speed, bool clipping) - */ - /**************************************************************************/ - void registerTelemetryCallback(void(*callbackTelemetry)(float, float, bool)); - - protected: - ServoState _state = UNDEFINED; - motorProperties *_motor; - machineGeometry *_physics; - float _travel; - int _minStep; - int _maxStep; - int _maxStepPerSecond; - int _maxStepAcceleration; - int _patternIndex = 0; - bool _isHomed = false; - int _index = 0; - int _depth; - int _previousDepth; - int _stroke; - int _previousStroke; - float _timeOfStroke; - float _sensation; - bool _applyUpdate = false; - static void _homingProcedureImpl(void* _this) { static_cast(_this)->_homingProcedure(); } - void _homingProcedure(); - static void _strokingImpl(void* _this) { static_cast(_this)->_stroking(); } - void _stroking(); - static void _streamingImpl(void* _this) { static_cast(_this)->_streaming(); } - void _streaming(); - TaskHandle_t _taskStrokingHandle = NULL; - TaskHandle_t _taskHomingHandle = NULL; - TaskHandle_t _taskStreamingHandle = NULL; - SemaphoreHandle_t _patternMutex = xSemaphoreCreateMutex(); - void _applyMotionProfile(motionParameter* motion); - void(*_callBackHomeing)(bool) = NULL; - void(*_callbackTelemetry)(float, float, bool) = NULL; - int _homeingSpeed; - int _homeingPin; - int _homeingToBack; - bool _homeingActiveLow; /*> Polarity of the homing signal*/ - bool _fancyAdjustment; - void _setupDepths(); + FastAccelStepper *_servo; + + public: + /**************************************************************************/ + /*! + @brief Initializes FastAccelStepper and configures all pins and outputs + accordingly. StrokeEngine is in state UNDEFINED + */ + /**************************************************************************/ + void begin(machineGeometry *physics, motorProperties *motor, + FastAccelStepper *servo); + + /**************************************************************************/ + /*! + @brief Set the speed of a stroke. Speed is given in Strokes per Minute + and internally calculated to the time a full stroke needs to complete. + Settings tale effect with next stroke, or after calling + applyNewSettingsNow(). + @param speed Strokes per Minute. Is constrained from 0.5 to 6000 + @param applyNow Set to true if changes should take effect immediately + */ + /**************************************************************************/ + void setSpeed(float speed, bool applyNow); + + /**************************************************************************/ + /*! + @brief Get the speed of a stroke. Speed is returned as Strokes per + Minute. + @return Strokes per Minute. + */ + /**************************************************************************/ + float getSpeed(); + + /**************************************************************************/ + /*! + @brief Set the depth of a stroke. Settings tale effect with next stroke, + or after calling applyNewSettingsNow(). + @param depth Depth in [mm]. Is constrained from 0 to TRAVEL + @param applyNow Set to true if changes should take effect immediately + */ + /**************************************************************************/ + void setDepth(float depth, bool applyNow); + + /**************************************************************************/ + /*! + @brief Returns the depth of a stroke. + @return Depth in [mm]. Is constrained from 0 to TRAVEL + */ + /**************************************************************************/ + float getDepth(); + + /**************************************************************************/ + /*! + @brief Set the stroke length of a stroke. Settings take effect with next + stroke, or after calling applyNewSettingsNow(). + @param stroke Stroke length in [mm]. Is constrained from 0 to TRAVEL + @param applyNow Set to true if changes should take effect immediately + */ + /**************************************************************************/ + void setStroke(float stroke, bool applyNow); + + /**************************************************************************/ + /*! + @brief Get the stroke length of a stroke. + @return Stroke length in [mm]. + */ + /**************************************************************************/ + float getStroke(); + + /**************************************************************************/ + /*! + @brief Set the sensation of a pattern. Sensation is an additional + parameter a pattern may use to alter its behaviour. Settings takes + effect with next stroke, or after calling applyNewSettingsNow(). + @param sensation Sensation in [a.u.]. Is constrained from -100 to 100 + with 0 beeing assumed as neutral. + @param applyNow Set to true if changes should take effect immediately + */ + /**************************************************************************/ + void setSensation(float sensation, bool applyNow); + + /**************************************************************************/ + /*! + @brief Get the sensation of a pattern. Sensation is an additional + parameter a pattern may use to alter its behaviour. + @return Sensation in [a.u.]. Is constrained from -100 to 100 + with 0 beeing assumed as neutral. + */ + /**************************************************************************/ + float getSensation(); + + /**************************************************************************/ + /*! + @brief Choose a pattern for the StrokeEngine. Settings take effect with + next stroke, or after calling applyNewSettingsNow(). + @param patternIndex Index of a pattern + @param applyNow Set to true if changes should take effect immediately + @return TRUE on success, FALSE, if patternIndex is invalid. Previous + pattern will be retained. + */ + /**************************************************************************/ + bool setPattern(Pattern *nextPattern, bool applyNow); + + /**************************************************************************/ + /*! + @brief Get the pattern index for the StrokeEngine. + @return Index of a pattern + */ + /**************************************************************************/ + int getPattern(); + + /**************************************************************************/ + /*! + @brief Creates a FreeRTOS task to run a stroking pattern. Only valid in + state READY. Pattern is initialized with the values from the set + functions. If the task is running, state is PATTERN. + @return TRUE when task was created and motion starts, FALSE on failure. + */ + /**************************************************************************/ + bool startPattern(); + + /**************************************************************************/ + /*! + @brief Stops the motion with MAX_ACCEL and deletes the stroking task. Is + in state READY afterwards. + */ + /**************************************************************************/ + void stopMotion(); + + /**************************************************************************/ + /*! + @brief Enable the servo/stepper and do the homing procedure. Drives + towards the endstop with HOMING_SPEED. Function is non-blocking and backed + by a task. Optionally a callback can be given to receive feedback if + homing succeeded going in state READY. If homing switch is not found after + traveling MAX_TRAVEL it times out, disables the servo and goes into + UNDEFINED. + @param endstop Pointer to a endstopProperties struct defining all relevant + properties like pin, pinmode, homing direction & signal + polarity. + @param speed Speed in mm/s used for finding the homing switch. + Defaults to 5.0 mm/s + @param callBackHoming Callback function is called after homing is done. + Function parametere holds a bool containing the success + (TRUE) or failure (FALSE) of homing. + */ + /**************************************************************************/ + void enableAndHome(endstopProperties *endstop, float speed = 5.0); + void enableAndHome(endstopProperties *endstop, void (*callBackHoming)(bool), + float speed = 5.0); + + /**************************************************************************/ + /*! + @brief If no homing switch is present homing can be done manually. Push + the endeffector all the way in and call thisIsHome(). This enables the + the servo and sets the position to -KEEPOUT_BOUNDARY + @param speed Speed in mm/s used for finding the homing switch. + Defaults to 5.0 mm/s + */ + /**************************************************************************/ + void thisIsHome(float speed = 5.0); + + /**************************************************************************/ + /*! + @brief In state PATTERN, SETUPDEPTH and READY this + moves the endeffector to TRAVEL. Can be used for adjustments. Stops any + running pattern and ends in state READY. + @param speed Speed in mm/s used for driving to max. + Defaults to 10.0 mm/s + @return TRUE on success, FALSE if state does not allow this. + */ + /**************************************************************************/ + bool moveToMax(float speed = 10.0); + + /**************************************************************************/ + /*! + @brief In state PATTERN, SETUPDEPTH and READY this + moves the endeffector to 0. Can be used for adjustments. Stops any running + pattern and ends in state READY. + @param speed Speed in mm/s used for driving to min. + Defaults to 10.0 mm/s + @return TRUE on success, FALSE if state does not allow this. + */ + /**************************************************************************/ + bool moveToMin(float speed = 10.0); + + /**************************************************************************/ + /*! + @brief In state PATTERN and READY this moves the endeffector + to DEPTH and enters state SETUPDEPTH. Follows the DEPTH postion + whenever setDepth() is called. Can be used for adjustments. Stops any + running pattern. + @param speed Speed in mm/s used for driving to min. + Defaults to 10.0 mm/s + @param fancy In fancy mode sensation allows to adjust both, depth and + stroke. +100 adjusts the depth position, -100 adjusts the + stroke position. 0 adjusts the midpoint depth-stroke/2. + @return TRUE on success, FALSE if state does not allow this. + */ + /**************************************************************************/ + bool setupDepth(float speed = 10.0, bool fancy = false); + + /**************************************************************************/ + /*! + @brief Retrieves the current servo state from the internal state machine. + @return Current state of the state machine + */ + /**************************************************************************/ + ServoState getState(); + + /**************************************************************************/ + /*! + @brief Disables the servo motor instantly and deletes any motion task. + Sets state machine to UNDEFINED. Must be followed by homing to enable + servo again. + */ + /**************************************************************************/ + void disable(); + + /**************************************************************************/ + /*! + @brief Makes the pattern list available for the main program to retreive + informations like pattern names. + @param index index of a pattern. + @return String holding a pattern name with a certain index. If index is + out of range it returns "Invalid" + */ + /**************************************************************************/ + String getPatternName(int index); + + /**************************************************************************/ + /*! + @brief Makes the pattern list available for the main program to retreive + informations like pattern names. + @return The number of pattern available. + */ + /**************************************************************************/ + unsigned int getNumberOfPattern() { return 0; }; + + /**************************************************************************/ + /*! + @brief Updates the maximum speed number of StrokeEngine. This value is + used to keep alle motions in check and as a safeguard. + @param maxSpeed maximum Speed in mm/s + */ + /**************************************************************************/ + void setMaxSpeed(float maxSpeed); + + /**************************************************************************/ + /*! + @brief Get the current set maximum speed + @return maximum speed in mm/s + */ + /**************************************************************************/ + float getMaxSpeed(); + + /**************************************************************************/ + /*! + @brief Updates the maximum acceleration number of StrokeEngine. This + value is used to keep alle motions in check and as a safeguard. + @param maxAcceleration maximum acceleration in mm/s² + */ + /**************************************************************************/ + void setMaxAcceleration(float maxAcceleration); + + /**************************************************************************/ + /*! + @brief Get the current set maximum acceleration + @return maximum acceleration in mm/s² + */ + /**************************************************************************/ + float getMaxAcceleration(); + + /**************************************************************************/ + /*! + @brief Register a callback function that will update telemetry + information about StrokeEngine. The provided function will be called + whenever a motion is executed by a manual command or by a pattern. The + returned values are the target position of this move, its top speed and + wether clipping occurred. + @param callbackTelemetry Function must be of type: + void callbackTelemetry(float position, float speed, bool clipping) + */ + /**************************************************************************/ + void registerTelemetryCallback(void (*callbackTelemetry)(float, float, + bool)); + + protected: + ServoState _state = UNDEFINED; + motorProperties *_motor; + machineGeometry *_physics; + float _travel; + int _minStep; + int _maxStep; + int _maxStepPerSecond; + int _maxStepAcceleration; + Pattern *pattern = new SimpleStroke("Simple Stroke"); + bool _isHomed = false; + int _index = 0; + int _depth; + int _previousDepth; + int _stroke; + int _previousStroke; + float _timeOfStroke; + float _sensation; + bool _applyUpdate = false; + static void _homingProcedureImpl(void *_this) { + static_cast(_this)->_homingProcedure(); + } + void _homingProcedure(); + static void _strokingImpl(void *_this) { + static_cast(_this)->_stroking(); + } + void _stroking(); + static void _streamingImpl(void *_this) { + static_cast(_this)->_streaming(); + } + void _streaming(); + TaskHandle_t _taskStrokingHandle = NULL; + TaskHandle_t _taskHomingHandle = NULL; + TaskHandle_t _taskStreamingHandle = NULL; + SemaphoreHandle_t _patternMutex = xSemaphoreCreateMutex(); + void _applyMotionProfile(motionParameter *motion); + void (*_callBackHomeing)(bool) = NULL; + void (*_callbackTelemetry)(float, float, bool) = NULL; + int _homeingSpeed; + int _homeingPin; + int _homeingToBack; + bool _homeingActiveLow; /*> Polarity of the homing signal*/ + bool _fancyAdjustment; + void _setupDepths(); }; diff --git a/Software/lib/StrokeEngine/src/pattern.h b/Software/lib/StrokeEngine/src/pattern.h index ef09b61f..1472b5fb 100644 --- a/Software/lib/StrokeEngine/src/pattern.h +++ b/Software/lib/StrokeEngine/src/pattern.h @@ -15,7 +15,6 @@ #include #include "PatternMath.h" -#include "StrokeEngine.h" #define DEBUG_PATTERN // Print some debug informations over Serial @@ -648,22 +647,3 @@ class Insist : public Pattern { _realStroke = int((float)_stroke * _strokeFraction); } }; - -/**************************************************************************/ -/* - Array holding all different patterns. Please include any custom pattern here. -*/ -/**************************************************************************/ -static Pattern *patternTable[] = { - new SimpleStroke("Simple Stroke"), - new TeasingPounding("Teasing or Pounding"), - new RoboStroke("Robo Stroke"), - new HalfnHalf("Half'n'Half"), - new Deeper("Deeper"), - new StopNGo("Stop'n'Go"), - new Insist("Insist") - // <-- insert your new pattern class here! -}; - -static const unsigned int patternTableSize = - sizeof(patternTable) / sizeof(patternTable[0]); diff --git a/Software/platformio.ini b/Software/platformio.ini index 35594ced..0e2f63e4 100644 --- a/Software/platformio.ini +++ b/Software/platformio.ini @@ -47,7 +47,6 @@ board = esp32dev framework = arduino monitor_speed = 115200 monitor_filters = colorize, esp32_exception_decoder, time -upload_port = /dev/cu.usbserial-0001 [env:debugWithoutMotor] build_flags = diff --git a/Software/src/constants/copy/en-us.h b/Software/src/constants/copy/en-us.h index 18b8326c..795aac69 100644 --- a/Software/src/constants/copy/en-us.h +++ b/Software/src/constants/copy/en-us.h @@ -42,7 +42,16 @@ static const LanguageStruct enUs = { "Stroke depth increases per cycle; sensation sets count.", "Pauses between strokes; sensation adjusts length.", "Modifies length, maintains speed; sensation influences direction." - } + }, + .StrokeEngineNames = { + "Simple Stroke", + "Teasing Pounding", + "Robo Stroke", + "Half'n'Half", + "Deeper", + "Stop'n'Go", + "Insist" + }, }; #endif // OSSM_SOFTWARE_EN_US_H diff --git a/Software/src/constants/copy/fr.h b/Software/src/constants/copy/fr.h index 114383c6..bcdee486 100644 --- a/Software/src/constants/copy/fr.h +++ b/Software/src/constants/copy/fr.h @@ -46,7 +46,17 @@ static const LanguageStruct fr = { "La profondeur des coups augmente à chaque cycle ; la sensation définit le nombre.", "Pauses entre les coups ; la sensation ajuste la longueur.", "Modifie la longueur, maintient la vitesse ; la sensation influe sur la direction.", - }}; + }, + .StrokeEngineNames = { + "Simple Stroke", + "Teasing Pounding", + "Robo Stroke", + "Half'n'Half", + "Deeper", + "Stop'n'Go", + "Insist", + } +}; #endif // OSSM_SOFTWARE_FR_H diff --git a/Software/src/main.cpp b/Software/src/main.cpp index 47c689c3..007a2332 100644 --- a/Software/src/main.cpp +++ b/Software/src/main.cpp @@ -39,15 +39,20 @@ void setup() { display.setBusClock(400000); display.begin(); - ossm = new OSSM(display, encoder); - + ossm = new OSSM(display, encoder, stepper); // link functions to be called on events. button.attachClick([]() { ossm->sm->process_event(ButtonPress{}); }); button.attachDoubleClick([]() { ossm->sm->process_event(DoublePress{}); }); button.attachLongPressStart([]() { ossm->sm->process_event(LongPress{}); }); + + xTaskCreate( + [](void* pvParameters) { + while (true) { + button.tick(); + vTaskDelay(10); + } + }, + "buttonTask", 4 * configMINIMAL_STACK_SIZE, nullptr, 1, nullptr); }; -void loop() { - button.tick(); - ossm->wm.process(); -}; \ No newline at end of file +void loop() { vTaskDelete(nullptr); }; \ No newline at end of file diff --git a/Software/src/ossm/OSSM.PatternControls.cpp b/Software/src/ossm/OSSM.PatternControls.cpp index d5f217b0..b68a2d9b 100644 --- a/Software/src/ossm/OSSM.PatternControls.cpp +++ b/Software/src/ossm/OSSM.PatternControls.cpp @@ -7,7 +7,7 @@ size_t numberOfDescriptions = sizeof(UserConfig::language.StrokeEngineDescriptions) / sizeof(UserConfig::language.StrokeEngineDescriptions[0]); -size_t numberOfPatterns = sizeof(patternTable) / sizeof(patternTable[0]); +size_t numberOfPatterns = 7; void OSSM::drawPatternControlsTask(void *pvParameters) { // parse ossm from the parameters @@ -19,9 +19,9 @@ void OSSM::drawPatternControlsTask(void *pvParameters) { return ossm->sm->is("strokeEngine.pattern"_s); }; - int nextPattern = ossm->setting.pattern; + int nextPattern = (int)ossm->setting.pattern; bool shouldUpdateDisplay = true; - String patternName = patternTable[nextPattern]->getName(); + String patternName = "nextPattern"; String patternDescription = UserConfig::language.StrokeEngineDescriptions[nextPattern]; @@ -33,13 +33,13 @@ void OSSM::drawPatternControlsTask(void *pvParameters) { while (isInCorrectState(ossm)) { nextPattern = ossm->encoder.readEncoder() / 3; shouldUpdateDisplay = - shouldUpdateDisplay || ossm->setting.pattern != nextPattern; + shouldUpdateDisplay || (int)ossm->setting.pattern != nextPattern; if (!shouldUpdateDisplay) { vTaskDelay(100); continue; } - patternName = patternTable[nextPattern]->getName(); + patternName = UserConfig::language.StrokeEngineNames[nextPattern]; if (nextPattern < numberOfDescriptions) { patternDescription = @@ -48,7 +48,7 @@ void OSSM::drawPatternControlsTask(void *pvParameters) { patternDescription = "No description available"; } - ossm->setting.pattern = nextPattern; + ossm->setting.pattern = (StrokePatterns)nextPattern; displayMutex.lock(); ossm->display.clearBuffer(); @@ -56,7 +56,7 @@ void OSSM::drawPatternControlsTask(void *pvParameters) { // Draw the title drawStr::title(patternName); drawStr::multiLine(0, 20, patternDescription); - drawShape::scroll(100* nextPattern / numberOfPatterns); + drawShape::scroll(100 * nextPattern / numberOfPatterns); ossm->display.sendBuffer(); displayMutex.unlock(); diff --git a/Software/src/ossm/OSSM.SimplePenetration.cpp b/Software/src/ossm/OSSM.SimplePenetration.cpp index 0b18073b..758f8d46 100644 --- a/Software/src/ossm/OSSM.SimplePenetration.cpp +++ b/Software/src/ossm/OSSM.SimplePenetration.cpp @@ -96,7 +96,7 @@ void OSSM::startSimplePenetrationTask(void *pvParameters) { } void OSSM::startSimplePenetration() { - int stackSize = 30 * configMINIMAL_STACK_SIZE; + int stackSize = 10 * configMINIMAL_STACK_SIZE; xTaskCreatePinnedToCore(startSimplePenetrationTask, "startSimplePenetrationTask", stackSize, this, diff --git a/Software/src/ossm/OSSM.StrokeEngine.cpp b/Software/src/ossm/OSSM.StrokeEngine.cpp index bed95aa5..59c521b9 100644 --- a/Software/src/ossm/OSSM.StrokeEngine.cpp +++ b/Software/src/ossm/OSSM.StrokeEngine.cpp @@ -1,6 +1,6 @@ #include "OSSM.h" -#include "../../lib/StrokeEngine/src/StrokeEngine.h" +#include "services/stepper.h" void OSSM::startStrokeEngineTask(void *pvParameters) { OSSM *ossm = (OSSM *)pvParameters; @@ -11,13 +11,11 @@ void OSSM::startStrokeEngineTask(void *pvParameters) { .keepoutBoundary = 6.0}; SettingPercents lastSetting = ossm->setting; - class StrokeEngine Stroker; - - Stroker.begin(&strokingMachine, &servoMotor, ossm->engine, ossm->stepper); + Stroker.begin(&strokingMachine, &servoMotor, ossm->stepper); Stroker.thisIsHome(); Stroker.setSensation(calculateSensation(ossm->setting.sensation), true); - Stroker.setPattern(int(ossm->setting.pattern), true); + Stroker.setDepth(0.01f * ossm->setting.depth * abs(measuredStrokeMm), true); Stroker.setStroke(0.01f * ossm->setting.stroke * abs(measuredStrokeMm), true); @@ -70,7 +68,35 @@ void OSSM::startStrokeEngineTask(void *pvParameters) { if (lastSetting.pattern != ossm->setting.pattern) { ESP_LOGD("UTILS", "change pattern: %d", ossm->setting.pattern); - Stroker.setPattern(ossm->setting.pattern, false); + + switch (ossm->setting.pattern) { + case StrokePatterns::SimpleStroke: + Stroker.setPattern(new SimpleStroke("Simple Stroke"), + false); + break; + case StrokePatterns::TeasingPounding: + Stroker.setPattern(new TeasingPounding("Teasing Pounding"), + false); + break; + case StrokePatterns::RoboStroke: + Stroker.setPattern(new RoboStroke("Robo Stroke"), false); + break; + case StrokePatterns::HalfnHalf: + Stroker.setPattern(new HalfnHalf("Half'n'Half"), false); + break; + case StrokePatterns::Deeper: + Stroker.setPattern(new Deeper("Deeper"), false); + break; + case StrokePatterns::StopNGo: + Stroker.setPattern(new StopNGo("Stop'n'Go"), false); + break; + case StrokePatterns::Insist: + Stroker.setPattern(new Insist("Insist"), false); + break; + default: + break; + } + lastSetting.pattern = ossm->setting.pattern; } @@ -83,7 +109,7 @@ void OSSM::startStrokeEngineTask(void *pvParameters) { } void OSSM::startStrokeEngine() { - int stackSize = 15 * configMINIMAL_STACK_SIZE; + int stackSize = 10 * configMINIMAL_STACK_SIZE; xTaskCreatePinnedToCore(startStrokeEngineTask, "startStrokeEngineTask", stackSize, this, configMAX_PRIORITIES - 1, diff --git a/Software/src/ossm/OSSM.cpp b/Software/src/ossm/OSSM.cpp index c89ea877..6ac7e2f8 100644 --- a/Software/src/ossm/OSSM.cpp +++ b/Software/src/ossm/OSSM.cpp @@ -11,26 +11,13 @@ using namespace sml; // Now we can define the OSSM constructor since OSSMStateMachine::operator() is // fully defined OSSM::OSSM(U8G2_SSD1306_128X64_NONAME_F_HW_I2C &display, - AiEsp32RotaryEncoder &encoder) + AiEsp32RotaryEncoder &encoder, FastAccelStepper *stepper) : display(display), encoder(encoder), + stepper(stepper), sm(std::make_unique< sml::sm, sml::logger>>(logger, *this)) { - engine.init(); - stepper = engine.stepperConnectToPin(Pins::Driver::motorStepPin); - if (stepper) { - stepper->setDirectionPin(Pins::Driver::motorDirectionPin, false); - stepper->setEnablePin(Pins::Driver::motorEnablePin, true); - stepper->setAutoEnable(false); - } - - // disable motor briefly in case we are against a hard stop. - digitalWrite(Pins::Driver::motorEnablePin, HIGH); - delay(600); - digitalWrite(Pins::Driver::motorEnablePin, LOW); - delay(100); - // NOTE: This is a hack to get the wifi credentials loaded early. wm.setConfigPortalBlocking(false); wm.startConfigPortal(); diff --git a/Software/src/ossm/OSSM.h b/Software/src/ossm/OSSM.h index 0c9a6718..74d2afbb 100644 --- a/Software/src/ossm/OSSM.h +++ b/Software/src/ossm/OSSM.h @@ -1,8 +1,6 @@ #ifndef OSSM_SOFTWARE_OSSM_H #define OSSM_SOFTWARE_OSSM_H -#include - #include "Actions.h" #include "AiEsp32RotaryEncoder.h" #include "Events.h" @@ -229,9 +227,7 @@ class OSSM { * //// * /////////////////////////////////////////// */ - FastAccelStepperEngine engine = FastAccelStepperEngine(); - FastAccelStepper *stepper = nullptr; - + FastAccelStepper *stepper; U8G2_SSD1306_128X64_NONAME_F_HW_I2C &display; StateLogger logger; AiEsp32RotaryEncoder &encoder; @@ -253,8 +249,11 @@ class OSSM { Menu menuOption; String errorMessage = ""; - SettingPercents setting = { - .speed = 0, .stroke = 0, .sensation = 50, .depth = 50, .pattern = 0}; + SettingPercents setting = {.speed = 0, + .stroke = 0, + .sensation = 50, + .depth = 50, + .pattern = StrokePatterns::SimpleStroke}; unsigned long sessionStartTime = 0; int sessionStrokeCount = 0; @@ -324,7 +323,8 @@ class OSSM { public: explicit OSSM(U8G2_SSD1306_128X64_NONAME_F_HW_I2C &display, - AiEsp32RotaryEncoder &rotaryEncoder); + AiEsp32RotaryEncoder &rotaryEncoder, + FastAccelStepper *stepper); std::unique_ptr< sml::sm, diff --git a/Software/src/services/board.h b/Software/src/services/board.h index 966e941a..2b4c9a7c 100644 --- a/Software/src/services/board.h +++ b/Software/src/services/board.h @@ -4,11 +4,14 @@ #include #include "constants/Pins.h" +#include "services/display.h" +#include "services/encoder.h" +#include "services/stepper.h" /** * This file changes the configuration of the board. */ -void initBoard() { +static void initBoard() { Serial.begin(115200); pinMode(Pins::Remote::encoderSwitch, @@ -26,6 +29,9 @@ void initBoard() { analogReadResolution(12); analogSetAttenuation(ADC_11db); // allows us to read almost full 3.3V range + initEncoder(); + initDisplay(); + initStepper(); } #endif // OSSM_SOFTWARE_BOARD_H diff --git a/Software/src/services/display.h b/Software/src/services/display.h index 731d611f..7aa4ee55 100644 --- a/Software/src/services/display.h +++ b/Software/src/services/display.h @@ -24,4 +24,10 @@ static U8G2_SSD1306_128X64_NONAME_F_HW_I2C display(U8G2_R0, Pins::Remote::displayClock, Pins::Remote::displayData); +static void initDisplay() { + // Display + display.setBusClock(400000); + display.begin(); +} + #endif // OSSM_SOFTWARE_DISPLAY_H diff --git a/Software/src/services/stepper.h b/Software/src/services/stepper.h index 9b01e485..073b9620 100644 --- a/Software/src/services/stepper.h +++ b/Software/src/services/stepper.h @@ -1,34 +1,27 @@ -#ifndef OSSM_SOFTWARE_STEPPER_H -#define OSSM_SOFTWARE_STEPPER_H +#ifndef SOFTWARE_STEPPER_H +#define SOFTWARE_STEPPER_H -#include - -#include "constants/Config.h" +#include "FastAccelStepper.h" #include "constants/Pins.h" -#include "services/tasks.h" - -/** - * Here are all the initialization steps for the flexyStepper motor. - * - * It is useful the call this function again if the motor is in an unknown - * state. - * - * @param flexyStepper - */ -static void initStepper(ESP_FlexyStepper &flexyStepper) { - flexyStepper.connectToPins(Pins::Driver::motorStepPin, - Pins::Driver::motorDirectionPin); - flexyStepper.setLimitSwitchActive(Pins::Driver::limitSwitchPin); - float stepsPerMm = - Config::Driver::motorStepPerRevolution / - (Config::Driver::pulleyToothCount * Config::Driver::beltPitchMm); +static FastAccelStepperEngine stepperEngine = FastAccelStepperEngine(); +static FastAccelStepper *stepper = nullptr; +static class StrokeEngine Stroker; - flexyStepper.setStepsPerMillimeter(stepsPerMm); +static void initStepper() { + stepperEngine.init(); + stepper = stepperEngine.stepperConnectToPin(Pins::Driver::motorStepPin); + if (stepper) { + stepper->setDirectionPin(Pins::Driver::motorDirectionPin, false); + stepper->setEnablePin(Pins::Driver::motorEnablePin, true); + stepper->setAutoEnable(false); + } - // This stepper must start on core 1, otherwise it may cause jitter. - // https://github.com/pkerspe/ESP-FlexyStepper#a-view-words-on-jitter-in-the-generated-step-signals - flexyStepper.startAsService(stepperCore); + // disable motor briefly in case we are against a hard stop. + digitalWrite(Pins::Driver::motorEnablePin, HIGH); + delay(600); + digitalWrite(Pins::Driver::motorEnablePin, LOW); + delay(100); } -#endif // OSSM_SOFTWARE_STEPPER_H +#endif // SOFTWARE_STEPPER_H diff --git a/Software/src/services/tasks.h b/Software/src/services/tasks.h index 07076383..ccb565d5 100644 --- a/Software/src/services/tasks.h +++ b/Software/src/services/tasks.h @@ -15,7 +15,6 @@ static TaskHandle_t drawHelloTaskH = nullptr; static TaskHandle_t drawMenuTaskH = nullptr; static TaskHandle_t drawPlayControlsTaskH = nullptr; static TaskHandle_t drawPatternControlsTaskH = nullptr; -static TaskHandle_t wmTaskH = nullptr; static TaskHandle_t drawPreflightTaskH = nullptr; static TaskHandle_t runHomingTaskH = nullptr; diff --git a/Software/src/structs/LanguageStruct.h b/Software/src/structs/LanguageStruct.h index 0fa7a18a..8a5847a4 100644 --- a/Software/src/structs/LanguageStruct.h +++ b/Software/src/structs/LanguageStruct.h @@ -31,6 +31,7 @@ struct LanguageStruct { String WiFiSetupLine2; String YouShouldNotBeHere; String StrokeEngineDescriptions[7]; + String StrokeEngineNames[7]; }; #endif // OSSM_SOFTWARE_LANGUAGESTRUCT_H diff --git a/Software/src/structs/SettingPercents.h b/Software/src/structs/SettingPercents.h index d1c38d2e..204290f4 100644 --- a/Software/src/structs/SettingPercents.h +++ b/Software/src/structs/SettingPercents.h @@ -1,12 +1,22 @@ #ifndef SOFTWARE_SETTINGPERCENTS_H #define SOFTWARE_SETTINGPERCENTS_H +enum class StrokePatterns { + SimpleStroke, + TeasingPounding, + RoboStroke, + HalfnHalf, + Deeper, + StopNGo, + Insist, +}; + struct SettingPercents { float speed; float stroke; float sensation; float depth; - int pattern; + StrokePatterns pattern; float speedKnob; }; diff --git a/Software/src/utils/StateLogger.h b/Software/src/utils/StateLogger.h index ba8a124e..a2de519c 100644 --- a/Software/src/utils/StateLogger.h +++ b/Software/src/utils/StateLogger.h @@ -49,9 +49,6 @@ struct StateLogger { ESP_LOGV(STATE_MACHINE_TAG, "%s", sml::aux::get_type_name()); // These are trace messages because lambda functions are very verbose. -// ESP_LOGV(STATE_MACHINE_TAG, "%s, %s", -// sml::aux::get_type_name(), -// sml::aux::get_type_name()); } template diff --git a/Software/src/utils/StrokeEngineHelper.h b/Software/src/utils/StrokeEngineHelper.h index bafb99d8..2c5fbd38 100644 --- a/Software/src/utils/StrokeEngineHelper.h +++ b/Software/src/utils/StrokeEngineHelper.h @@ -13,11 +13,7 @@ ##################################################################################################*/ // enum of stroke engine states -enum PlayControls { - STROKE, - DEPTH, - SENSATION -}; +enum PlayControls { STROKE, DEPTH, SENSATION }; static motorProperties servoMotor{ .maxSpeed = @@ -42,4 +38,5 @@ static float calculateSensation(float sensationPercentage) { return float((sensationPercentage * 200.0) / 100.0) - 100.0f; } + #endif \ No newline at end of file diff --git a/Software/src/utils/update.h b/Software/src/utils/update.h index 964581d4..2b6edee4 100644 --- a/Software/src/utils/update.h +++ b/Software/src/utils/update.h @@ -96,6 +96,8 @@ auto updateOSSM = []() { ESP_LOGD("UTILS", "HTTP_UPDATE_OK"); break; } + + client.stop(); }; #endif // SOFTWARE_UPDATE_H