From de42b6252ebec3558713cd39334548041654545f Mon Sep 17 00:00:00 2001 From: jonny_ji7 Date: Sun, 12 Mar 2023 13:06:30 +0100 Subject: [PATCH] Stepper: Add resume/update functionality (WIP) add way to extend current move operation without having to wait for IDLE state (full decel time) --- components/DendoStepper/DendoStepper.cpp | 92 ++++++++++++++++--- .../DendoStepper/include/DendoStepper.h | 8 +- main/guide-stepper.cpp | 27 +++++- 3 files changed, 107 insertions(+), 20 deletions(-) diff --git a/components/DendoStepper/DendoStepper.cpp b/components/DendoStepper/DendoStepper.cpp index 6443845..07d31aa 100644 --- a/components/DendoStepper/DendoStepper.cpp +++ b/components/DendoStepper/DendoStepper.cpp @@ -116,21 +116,35 @@ timer_avail: esp_err_t DendoStepper::runPos(int32_t relative) { - if (!relative) // why would u call it with 0 wtf - return ESP_ERR_NOT_SUPPORTED; - if (ctrl.status > IDLE) - { // we are running, we need to adjust steps accordingly, for now just stop the movement - STEP_LOGW("DendoStepper", "Finising previous move, this command will be ignored"); - return ESP_ERR_NOT_SUPPORTED; - } - + //TODO only enable when actually moving if (ctrl.status == DISABLED) // if motor is disabled, enable it enableMotor(); - ctrl.status = ACC; - setDir(relative < 0); // set CCW if <0, else set CW - currentPos += relative; - calc(abs(relative)); // calculate velocity profile + + if (!relative) // why would u call it with 0 wtf + return ESP_ERR_NOT_SUPPORTED; + + if (ctrl.status > IDLE) { //currently moving + bool newDir = (relative < 0); // CCW if <0, else set CW + if (ctrl.dir == newDir){ //current direction is the same + ctrl.statusPrev = ctrl.status; //update previous status + ctrl.status = ctrl.status==COAST ? COAST : ACC; //stay at coast otherwise switch to ACC + calc(abs(relative + ctrl.stepsRemaining)); //calculate new velolcity profile for new+remaining steps + } else { //direction has changed + //direction change not supported TODO wait for decel finish / queue? + STEP_LOGW("DendoStepper", "DIRECTION HOT-CHANGE NOT SUPPORTED - Finising previous move, this command will be ignored"); + return ESP_ERR_NOT_SUPPORTED; + } + } + else { //current state is IDLE + ctrl.statusPrev = ctrl.status; //update previous status + ctrl.status = ACC; + setDir(relative < 0); // set CCW if <0, else set CW + calc(abs(relative)); // calculate velocity profile + } + + currentPos += relative; //(target position / not actual) ESP_ERROR_CHECK(timer_set_alarm_value(conf.timer_group, conf.timer_idx, ctrl.stepInterval)); // set HW timer alarm to stepinterval + //TODO timer has to be stopped before update if already running? ESP_ERROR_CHECK(timer_start(conf.timer_group, conf.timer_idx)); // start the timer return ESP_OK; @@ -195,6 +209,37 @@ void DendoStepper::setSpeedMm(uint32_t speed, uint16_t accT, uint16_t decT) STEP_LOGI("DendoStepper", "Speed set: v=%d mm/s t+=%d s t-=%d s", speed, accT, decT); } +//CUSTOM - change speed while running +//FIXME: this approach does not work, since calc function would have to be run after change, this will mess up target steps... +//void DendoStepper::changeSpeed(uint32_t speed) +//{ +// //TODO reduce duplicate code (e.g. call setSpeed function) +// //change speed +// ctrl.speed = speed; +// //change status to ACC/DEC +// STEP_LOGI("DendoStepper", "Speed changed: from v=%.2f rad/s to v=%.2f rad/s", ctrl.speed, speed); +// if (speed > ctrl.speed) ctrl.status = ACC; +// if (speed < ctrl.speed) ctrl.status = DEC; +//} +// +//void DendoStepper::changeSpeedMm(uint32_t speed) +//{ +// //TODO reduce duplicate code (e.g. call setSpeedMm function) +// if (ctrl.stepsPerMm == 0) +// { +// STEP_LOGE("DendoStepper", "Steps per millimeter not set, cannot set the speed!"); +// } +// //calc new speed +// float speedNew = speed * ctrl.stepsPerMm; +// //change status to ACC/DEC +// if (speedNew > ctl.speed) ctrl.status = ACC; +// if (speedNew < ctl.speed) ctrl.status = DEC; +// //update speed, log output +// ctrl.speed = speedNew; +// STEP_LOGI("DendoStepper", "Speed changed: from v=%.2f rad/s to v=%.2f rad/s", ctrl.speed, speedNew); +//} + + void DendoStepper::setStepsPerMm(uint16_t steps) { ctrl.stepsPerMm = steps; @@ -261,6 +306,7 @@ void DendoStepper::stop() } ctrl.runInfinite = false; timer_pause(conf.timer_group, conf.timer_idx); // stop the timer + ctrl.statusPrev = ctrl.status; //update previous status ctrl.status = IDLE; ctrl.stepCnt = 0; gpio_set_level((gpio_num_t)conf.stepPin, 0); @@ -287,10 +333,20 @@ bool DendoStepper::xISR() ctrl.stepCnt++; + //CUSTOM: track actual precice current position + if (ctrl.dir) { + ctrl.posActual ++; + } else { + ctrl.posActual --; + } + //CUSTOM: track remaining steps for eventually resuming + ctrl.stepsRemaining = ctrl.stepCnt - ctrl.stepCnt; + // we are done if (ctrl.stepsToGo == ctrl.stepCnt && !ctrl.runInfinite) { timer_pause(conf.timer_group, conf.timer_idx); // stop the timer + ctrl.statusPrev = ctrl.status; //update previous status ctrl.status = IDLE; ctrl.stepCnt = 0; return 0; @@ -299,16 +355,19 @@ bool DendoStepper::xISR() if (ctrl.stepCnt > 0 && ctrl.stepCnt < ctrl.accEnd) { // we are accelerating ctrl.currentSpeed += ctrl.accInc; + ctrl.statusPrev = ctrl.status; //update previous status ctrl.status = ACC; // we are accelerating, note that*/ } else if (ctrl.stepCnt > ctrl.coastEnd && !ctrl.runInfinite) { // we must be deccelerating then ctrl.currentSpeed -= ctrl.decInc; + ctrl.statusPrev = ctrl.status; //update previous status ctrl.status = DEC; // we are deccelerating } else { ctrl.currentSpeed = ctrl.targetSpeed; + ctrl.statusPrev = ctrl.status; //update previous status ctrl.status = COAST; // we are coasting } @@ -316,15 +375,18 @@ bool DendoStepper::xISR() // set alarm to calculated interval and disable pin GPIO.out_w1tc = (1ULL << conf.stepPin); timer_set_alarm_value(conf.timer_group, conf.timer_idx, ctrl.stepInterval); + ctrl.stepCnt++; return 1; } void DendoStepper::calc(uint32_t targetSteps) { - - ctrl.accSteps = 0.5 * ctrl.acc * (ctrl.speed / ctrl.acc) * (ctrl.speed / ctrl.acc); - + //steps from ctrl.speed -> 0: ctrl.decSteps = 0.5 * ctrl.dec * (ctrl.speed / ctrl.dec) * (ctrl.speed / ctrl.dec); + //steps from 0 -> ctrl.speed: + //ctrl.accSteps = 0.5 * ctrl.acc * (ctrl.speed / ctrl.acc) * (ctrl.speed / ctrl.acc); + //steps from ctrl.currentSpeed -> ctrl.speed: + ctrl.accSteps = 0.5 * ctrl.acc * (ctrl.speed / ctrl.acc) * (ctrl.speed / ctrl.acc) * (ctrl.speed - ctrl.currentSpeed) / ctrl.speed; if (targetSteps < (ctrl.decSteps + ctrl.accSteps)) { diff --git a/components/DendoStepper/include/DendoStepper.h b/components/DendoStepper/include/DendoStepper.h index 9846412..a327ecc 100644 --- a/components/DendoStepper/include/DendoStepper.h +++ b/components/DendoStepper/include/DendoStepper.h @@ -94,6 +94,9 @@ typedef struct float dec = 100; // decceleration in rad*second^-2 uint32_t accSteps = 0; uint32_t decSteps = 0; + int32_t stepsRemaining = 0; + uint64_t posActual = 0; //actual current pos incremented at every step + uint8_t statusPrev = DISABLED; //FIXME currently unused uint8_t status = DISABLED; bool dir = CW; bool runInfinite = false; @@ -208,6 +211,9 @@ public: */ void setSpeedMm(uint32_t speed, uint16_t accT, uint16_t decT); + //CUSTOM: change speed while running + void changeSpeedMm(uint32_t speed); + /** * @brief Set steps per 1 mm of linear movement * @@ -272,4 +278,4 @@ public: void stop(); }; -#endif \ No newline at end of file +#endif diff --git a/main/guide-stepper.cpp b/main/guide-stepper.cpp index 37cd5de..8dbd96b 100644 --- a/main/guide-stepper.cpp +++ b/main/guide-stepper.cpp @@ -50,6 +50,7 @@ static DendoStepper step; static const char *TAG = "stepper"; //tag for logging static bool stepp_direction = true; +static bool dir = true, dirPrev; //TODO local variables in travelSteps? static uint32_t posNow = 0; @@ -65,13 +66,23 @@ void travelSteps(int stepsTarget){ stepsToGo = abs(stepsTarget); if(stepsTarget < 0) stepp_direction = !stepp_direction; //invert direction in reverse mode + while (stepsToGo != 0){ + + //--- wait if direction changed --- + if (dirPrev != dir){ + ESP_LOGI(TAG, " dir-change detected - waiting for move to finish \n "); + while(step.getState() != 1) vTaskDelay(1); //wait for move to finish + } + //--- currently moving right --- if (stepp_direction == true){ //currently moving right remaining = POS_MAX_STEPS - posNow; //calc remaining distance fom current position to limit if (stepsToGo > remaining){ //new distance will exceed limit step.runAbs (POS_MAX_STEPS); //move to limit - while(step.getState() != 1) vTaskDelay(1); //wait for move to finish + dirPrev = dir; + dir = 1; + //while(step.getState() != 1) vTaskDelay(1); //wait for move to finish posNow = POS_MAX_STEPS; stepp_direction = false; //change current direction for next iteration stepsToGo = stepsToGo - remaining; //decrease target length by already traveled distance @@ -79,7 +90,10 @@ void travelSteps(int stepsTarget){ } else { //target distance does not reach the limit step.runAbs (posNow + stepsToGo); //move by (remaining) distance to reach target length - while(step.getState() != 1) vTaskDelay(1); //wait for move to finish + dirPrev = dir; + dir = 1; + //-- dont wait for move to finish since moves in same direction get merged -- + //while(step.getState() != 1) vTaskDelay(1); //wait for move to finish ESP_LOGD(TAG, "moving to %d\n", posNow+stepsToGo); posNow += stepsToGo; stepsToGo = 0; //finished, reset target length (could as well exit loop/break) @@ -91,7 +105,9 @@ void travelSteps(int stepsTarget){ remaining = posNow - POS_MIN_STEPS; if (stepsToGo > remaining){ step.runAbs (POS_MIN_STEPS); - while(step.getState() != 1) vTaskDelay(2); //wait for move to finish + dirPrev = dir; + dir = 0; + //while(step.getState() != 1) vTaskDelay(2); //wait for move to finish posNow = POS_MIN_STEPS; stepp_direction = true; stepsToGo = stepsToGo - remaining; @@ -99,7 +115,10 @@ void travelSteps(int stepsTarget){ } else { step.runAbs (posNow - stepsToGo); //when moving left the coordinate has to be decreased - while(step.getState() != 1) vTaskDelay(2); //wait for move to finish + dirPrev = dir; + dir = 0; + //-- dont wait for move to finish since moves in same direction get merged -- + //while(step.getState() != 1) vTaskDelay(2); //wait for move to finish ESP_LOGD(TAG, "moving to %d\n", posNow - stepsToGo); posNow -= stepsToGo; stepsToGo = 0;