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

Stepper: rework resume functionality

Browse Source
This commit is contained in:
jonny_ji7 2023-03-13 20:02:22 +01:00
parent 0a05340763
commit dc6deeb3d0
2 changed files with 39 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
components/DendoStepper/DendoStepper.cpp
View File

@ -117,41 +117,35 @@ timer_avail:
ESP_ERROR_CHECK(timer_isr_callback_add(conf.timer_group, conf.timer_idx, xISRwrap, this, 0)); // add callback fn to run when alarm is triggrd ESP_ERROR_CHECK(timer_isr_callback_add(conf.timer_group, conf.timer_idx, xISRwrap, this, 0)); // add callback fn to run when alarm is triggrd
} }
esp_err_t DendoStepper::runPos(int32_t relative) esp_err_t DendoStepper::runPos(int32_t relative)
{ {
//TODO only enable when actually moving //TODO only enable when actually moving
if (ctrl.status == DISABLED) // if motor is disabled, enable it if (ctrl.status == DISABLED) // if motor is disabled, enable it
enableMotor(); enableMotor();
bool newDir = (relative < 0); // CCW if <0, else set CW setDir(relative < 0); // set CCW if <0, else set CW
printf("runpos start -- steps: %d, remaining: %d, status: %d, olddir: %d, newdir: %d\n", relative, ctrl.stepsRemaining, ctrl.status, ctrl.dir, newDir);
if (!relative) // why would u call it with 0 wtf if (!relative) // why would u call it with 0 wtf
return ESP_ERR_NOT_SUPPORTED; return ESP_ERR_NOT_SUPPORTED;
if (ctrl.status > IDLE) { //currently moving if (ctrl.status > IDLE) { //currently moving
if (ctrl.dir == newDir){ //current direction is the same ctrl.status = ctrl.status==COAST ? COAST : ACC; //stay at coast otherwise switch to ACC
//ctrl.statusPrev = ctrl.status; //update previous status ctrl.stepsRemaining = ctrl.stepsToGo - ctrl.stepCnt;
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
ctrl.stepsRemaining = ctrl.stepsToGo - ctrl.stepCnt; ESP_LOGW("DendoStepper", "EXTEND running movement (stepsRemaining: %d + stepsNew: %d)", ctrl.stepsRemaining, relative);
calc(abs(relative + ctrl.stepsRemaining)); //calculate new velolcity profile for new+remaining steps ESP_ERROR_CHECK(timer_set_alarm_value(conf.timer_group, conf.timer_idx, ctrl.stepInterval)); // set HW timer alarm to stepinterval
ESP_LOGW("DendoStepper", "EXTEND running movement (stepsRemaining: %d + stepsNew: %d)", ctrl.stepsRemaining, relative);
ESP_ERROR_CHECK(timer_set_alarm_value(conf.timer_group, conf.timer_idx, ctrl.stepInterval)); // set HW timer alarm to stepinterval
} else { //direction has changed
//direction change not supported TODO wait for decel finish / queue?
STEP_LOGE("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
ESP_ERROR_CHECK(timer_set_alarm_value(conf.timer_group, conf.timer_idx, ctrl.stepInterval)); // set HW timer alarm to stepinterval
ESP_ERROR_CHECK(timer_start(conf.timer_group, conf.timer_idx)); // start the timer
} }
printf("runpos end -- steps: %d, status: %d, olddir: %d, newdir: %d\n", relative, ctrl.status, ctrl.dir, newDir); else { //current state is IDLE
//ctrl.statusPrev = ctrl.status; //update previous status
ctrl.status = ACC;
calc(abs(relative)); // calculate velocity profile
ESP_ERROR_CHECK(timer_set_alarm_value(conf.timer_group, conf.timer_idx, ctrl.stepInterval)); // set HW timer alarm to stepinterval
ESP_ERROR_CHECK(timer_start(conf.timer_group, conf.timer_idx)); // start the timer
}
//printf("runpos end -- steps: %d, status: %d, olddir: %d, newdir: %d\n", relative, ctrl.status, ctrl.dir, newDir);
currentPos += relative; //(target position / not actual) currentPos += relative; //(target position / not actual)
return ESP_OK; return ESP_OK;
} }
@ -165,26 +159,31 @@ esp_err_t DendoStepper::runPosMm(int32_t relative)
return runPos(relative * ctrl.stepsPerMm); return runPos(relative * ctrl.stepsPerMm);
} }
//customized: if already running and direction is the same immediately pass to runPos
esp_err_t DendoStepper::runAbs(uint32_t position) esp_err_t DendoStepper::runAbs(uint32_t position)
{ {
//if (getState() > IDLE) // we are already moving, so stop it //exit if nothing to do
// stop(); if (position == currentPos) return 0; //already at position
//while (getState() > IDLE)
//{
// // waiting for idle, watchdog should take care of inf loop if it occurs
// vTaskDelay(1);
//} // shouldnt take long tho
if (position == currentPos) // we cant go anywhere
return 0;
//calculate steps necessary
int32_t relativeSteps = 0; int32_t relativeSteps = 0;
relativeSteps = (int32_t)position - currentPos; relativeSteps = (int32_t)position - currentPos;
//wait if direction needs to change
if (getState() > IDLE){//already moving
bool newDir = (relativeSteps < 0); // CCW if <0, else set CW
if (ctrl.dir != newDir){ //direction differs
STEP_LOGE("DendoStepper", "DIRECTION HOT-CHANGE NOT SUPPORTED - Waiting for move to finish...");
while (getState() > IDLE) vTaskDelay(1); //wait for move to finish
}
}
//call runPos with new target position
ESP_LOGI("DendoStepper", "Cur: %llu move %d", currentPos, relativeSteps); ESP_LOGI("DendoStepper", "Cur: %llu move %d", currentPos, relativeSteps);
return runPos(relativeSteps); // run to new position return runPos(relativeSteps); // run to new position
} }
esp_err_t DendoStepper::runAbsMm(uint32_t position) esp_err_t DendoStepper::runAbsMm(uint32_t position)
{ {
if (ctrl.stepsPerMm == 0) if (ctrl.stepsPerMm == 0)
@ -404,7 +403,8 @@ void DendoStepper::calc(uint32_t targetSteps)
ESP_LOGI("Dendostepper", "Computing new speed"); ESP_LOGI("Dendostepper", "Computing new speed");
ctrl.speed = sqrt(2 * targetSteps * ((ctrl.dec * ctrl.acc) / (ctrl.dec + ctrl.acc))); ctrl.speed = sqrt(2 * targetSteps * ((ctrl.dec * ctrl.acc) / (ctrl.dec + ctrl.acc)));
ctrl.accSteps = 0.5 * ctrl.acc * (ctrl.speed / ctrl.acc) * (ctrl.speed / ctrl.acc); //ctrl.accSteps = 0.5 * ctrl.acc * (ctrl.speed / ctrl.acc) * (ctrl.speed / ctrl.acc);
ctrl.accSteps = 0.5 * ctrl.acc * (ctrl.speed / ctrl.acc) * (ctrl.speed / ctrl.acc) * (ctrl.speed - ctrl.currentSpeed) / ctrl.speed;
ctrl.decSteps = 0.5 * ctrl.dec * (ctrl.speed / ctrl.dec) * (ctrl.speed / ctrl.dec); ctrl.decSteps = 0.5 * ctrl.dec * (ctrl.speed / ctrl.dec) * (ctrl.speed / ctrl.dec);
} }
@ -412,7 +412,7 @@ void DendoStepper::calc(uint32_t targetSteps)
ctrl.coastEnd = targetSteps - ctrl.decSteps; ctrl.coastEnd = targetSteps - ctrl.decSteps;
ctrl.targetSpeed = ctrl.speed; ctrl.targetSpeed = ctrl.speed;
ctrl.accInc = ctrl.targetSpeed / (double)ctrl.accSteps; ctrl.accInc = (ctrl.targetSpeed - ctrl.currentSpeed) / (double)ctrl.accSteps;
ctrl.decInc = ctrl.targetSpeed / (double)ctrl.decSteps; ctrl.decInc = ctrl.targetSpeed / (double)ctrl.decSteps;
ctrl.currentSpeed = ctrl.accInc; ctrl.currentSpeed = ctrl.accInc;

8
main/guide-stepper.cpp
View File

@ -71,10 +71,10 @@ void travelSteps(int stepsTarget){
while (stepsToGo != 0){ while (stepsToGo != 0){
//--- wait if direction changed --- //--- wait if direction changed ---
if (dirPrev != dir){ //if (dirPrev != dir){
ESP_LOGW(TAG, " dir-change detected - waiting for move to finish \n "); // ESP_LOGW(TAG, " dir-change detected - waiting for move to finish \n ");
while(step.getState() != 1) vTaskDelay(1); //wait for move to finish // while(step.getState() != 1) vTaskDelay(1); //wait for move to finish
} //}
//--- currently moving right --- //--- currently moving right ---
if (stepp_direction == true){ //currently moving right if (stepp_direction == true){ //currently moving right