j/cable-length-cutter
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Stepper: Add resume/update functionality (WIP)

Browse Source 
add way to extend current move operation without having to wait for IDLE
state (full decel time)
This commit is contained in:
jonny_ji7 2023-03-12 13:06:30 +01:00
parent 423348be9f
commit de42b6252e
3 changed files with 107 additions and 20 deletions
components/DendoStepper
DendoStepper.cpp
include
DendoStepper.h
main
guide-stepper.cpp

92
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))
{

8
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
#endif

27
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;