diff --git a/components/DendoStepper/DendoStepper.cpp b/components/DendoStepper/DendoStepper.cpp index 33126f5..cc64144 100644 --- a/components/DendoStepper/DendoStepper.cpp +++ b/components/DendoStepper/DendoStepper.cpp @@ -132,15 +132,15 @@ esp_err_t DendoStepper::runPos(int32_t relative) if (ctrl.status > IDLE) { //currently moving //ctrl.status = ctrl.status==COAST ? COAST : ACC; //stay at coast otherwise switch to ACC ctrl.stepsRemaining = ctrl.stepsToGo - ctrl.stepCnt; - calc(abs(relative + ctrl.stepsRemaining)); //calculate new velolcity profile for new+remaining steps - 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_LOGW("DendoStepper", "EXTEND running movement (stepsRemaining: %d + stepsNew: %d - current state: %d)", ctrl.stepsRemaining, abs(relative), (int)ctrl.status); ESP_ERROR_CHECK(timer_set_alarm_value(conf.timer_group, conf.timer_idx, ctrl.stepInterval)); // set HW timer alarm to stepinterval } else { //current state is IDLE //ctrl.statusPrev = ctrl.status; //update previous status - ctrl.status = ACC; calc(abs(relative)); // calculate velocity profile + ctrl.status = ACC; 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 } @@ -388,8 +388,16 @@ bool DendoStepper::xISR() void DendoStepper::calc(uint32_t targetSteps) { + //only set initial speed if IDLE + if(ctrl.status == 1){ + ctrl.currentSpeed = 0; + ESP_LOGD("DendoStepper", "calc-start: reset speed to 0 (start from idle) %lf\n", ctrl.currentSpeed); + } + else{ + ESP_LOGD("DendoStepper", "calc start: NOT resetting speed (extend from ACC/DEC/COAST): %lf\n", ctrl.currentSpeed); + } //CUSTOM reset counter if already moving - ctrl.stepCnt = 0; + ctrl.stepCnt = 0; //FIXME bugs when set 0 while ISR reads/runs? mutex //steps from ctrl.speed -> 0: ctrl.decSteps = 0.5 * ctrl.dec * (ctrl.speed / ctrl.dec) * (ctrl.speed / ctrl.dec); @@ -397,6 +405,7 @@ void DendoStepper::calc(uint32_t targetSteps) //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; + ESP_LOGD("DendoStepper", "accSteps: %d currspeed: %lf, ctrlSpeed: %lf\n", ctrl.accSteps, ctrl.currentSpeed, ctrl.speed); if (targetSteps < (ctrl.decSteps + ctrl.accSteps)) { @@ -415,12 +424,21 @@ void DendoStepper::calc(uint32_t targetSteps) ctrl.accInc = (ctrl.targetSpeed - ctrl.currentSpeed) / (double)ctrl.accSteps; ctrl.decInc = ctrl.targetSpeed / (double)ctrl.decSteps; - ctrl.currentSpeed = ctrl.accInc; + //only set initial speed if IDLE + if(ctrl.status == 1){ + ctrl.currentSpeed = ctrl.accInc; + ESP_LOGD("DendoStepper", "`reset curr speeed to accinc: %lf\n", ctrl.currentSpeed); + ESP_LOGD("DendoStepper", "status=%d setting speed to initial value: %lf\n",ctrl.status, ctrl.currentSpeed); + } + else{ + ESP_LOGD("DendoStepper", "status=%d NOT resetting speed to initial value %lf\n",ctrl.status, ctrl.currentSpeed); + } ctrl.stepInterval = TIMER_F / ctrl.currentSpeed; ctrl.stepsToGo = targetSteps; - printf("CALC: speedNow=%.1f, speedTarget=%.1f, accEnd=%d, coastEnd=%d, accSteps=%d, accInc=%.3f\n", + ESP_LOGD("DendoStepper", "DEBUG: accSteps: %d currspeed: %lf, ctrlSpeed: %lf\n", ctrl.accSteps, ctrl.currentSpeed, ctrl.speed); + ESP_LOGD("DendoStepper", "CALC: speedNow=%.1f, speedTarget=%.1f, accEnd=%d, coastEnd=%d, accSteps=%d, accInc=%.3f\n", ctrl.currentSpeed, ctrl.targetSpeed, ctrl.accEnd, ctrl.coastEnd, ctrl.accSteps, ctrl.accInc); STEP_LOGI("calc", "acc end:%u coastend:%u stepstogo:%u speed:%f acc:%f int: %u", ctrl.accEnd, ctrl.coastEnd, ctrl.stepsToGo, ctrl.speed, ctrl.acc, ctrl.stepInterval); } diff --git a/main/guide-stepper.cpp b/main/guide-stepper.cpp index fb6605b..44e4e60 100644 --- a/main/guide-stepper.cpp +++ b/main/guide-stepper.cpp @@ -33,8 +33,8 @@ extern "C" #define SPEED_MAX 60.0 //mm/s #define SPEED 10 //35, 100, 50 rev -#define ACCEL_MS 1000.0 //ms from 0 to max -#define DECEL_MS 1000.0 //ms from max to 0 +#define ACCEL_MS 800.0 //ms from 0 to max +#define DECEL_MS 800.0 //ms from max to 0 #define STEPPER_STEPS_PER_ROT 1600 #define STEPPER_STEPS_PER_MM STEPPER_STEPS_PER_ROT/4 @@ -219,19 +219,21 @@ void task_stepper_test(void *pvParameter) SW_AUTO_CUT.handle(); if (SW_RESET.risingEdge) { + ESP_LOGI(TAG, "button - stop stepper\n "); buzzer.beep(1, 1000, 100); step.stop(); - ESP_LOGI(TAG, "button - stop stepper\n "); } if (SW_PRESET1.risingEdge) { - buzzer.beep(2, 300, 100); - step.runPosMm(30); ESP_LOGI(TAG, "button - moving right\n "); + buzzer.beep(2, 300, 100); + step.setSpeedMm(SPEED, ACCEL_MS, DECEL_MS); + step.runPosMm(15); } if (SW_PRESET3.risingEdge) { - buzzer.beep(1, 500, 100); - step.runPosMm(-30); ESP_LOGI(TAG, "button - moving left\n "); + buzzer.beep(1, 500, 100); + step.setSpeedMm(SPEED, ACCEL_MS, DECEL_MS); + step.runPosMm(-15); } if (SW_PRESET2.risingEdge) { buzzer.beep(1, 100, 100); diff --git a/main/main.cpp b/main/main.cpp index 3b7494b..af2f72a 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -87,7 +87,7 @@ extern "C" void app_main() esp_log_level_set("switches-analog", ESP_LOG_WARN); esp_log_level_set("control", ESP_LOG_INFO); esp_log_level_set("stepper", ESP_LOG_DEBUG); - esp_log_level_set("Dendostepper", ESP_LOG_WARN); //stepper lib + esp_log_level_set("Dendostepper", ESP_LOG_DEBUG); //stepper lib esp_log_level_set("calc", ESP_LOG_WARN); //stepper lib #ifdef STEPPER_TEST