revert mutex, fix state reset, logging, slower
stepsRemaining not necessary in ISR comment out forced stop at runAbs statusPrev not needed TODO: ISR defines state every time, no need to adjust manually while running adjust calc function to handle remaining steps Test: broken - slow moves mork but when "extending" movement it just vibrates
This commit is contained in:
jonny_l480
parent
45409676a0
commit
0a05340763
@ -55,6 +55,9 @@ void DendoStepper::init(uint8_t stepP, uint8_t dirP, uint8_t enP, timer_group_t
|
||||
|
||||
void DendoStepper::init()
|
||||
{
|
||||
ESP_LOGW("DendoStepper", "semaphore init");
|
||||
semaphore_isrVariables = xSemaphoreCreateBinary();
|
||||
xSemaphoreGive(semaphore_isrVariables);
|
||||
uint64_t mask = (1ULL << conf.stepPin) | (1ULL << conf.dirPin) | (1ULL << conf.enPin); // put output gpio pins in bitmask
|
||||
gpio_config_t gpio_conf = {
|
||||
// config gpios
|
||||
@ -120,18 +123,19 @@ esp_err_t DendoStepper::runPos(int32_t relative)
|
||||
if (ctrl.status == DISABLED) // if motor is disabled, enable it
|
||||
enableMotor();
|
||||
|
||||
bool newDir = (relative < 0); // 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
|
||||
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.statusPrev = ctrl.status; //update previous status
|
||||
ctrl.status = ctrl.status==COAST ? COAST : ACC; //stay at coast otherwise switch to ACC
|
||||
xSemaphoreTake(semaphore_isrVariables, portMAX_DELAY);
|
||||
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);
|
||||
xSemaphoreGive(semaphore_isrVariables);
|
||||
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");
|
||||
@ -139,17 +143,16 @@ esp_err_t DendoStepper::runPos(int32_t relative)
|
||||
}
|
||||
}
|
||||
else { //current state is IDLE
|
||||
ctrl.statusPrev = ctrl.status; //update previous status
|
||||
//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);
|
||||
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;
|
||||
}
|
||||
|
||||
@ -164,13 +167,13 @@ esp_err_t DendoStepper::runPosMm(int32_t relative)
|
||||
|
||||
esp_err_t DendoStepper::runAbs(uint32_t position)
|
||||
{
|
||||
if (getState() > IDLE) // we are already moving, so stop it
|
||||
stop();
|
||||
while (getState() > IDLE)
|
||||
{
|
||||
// waiting for idle, watchdog should take care of inf loop if it occurs
|
||||
vTaskDelay(1);
|
||||
} // shouldnt take long tho
|
||||
//if (getState() > IDLE) // we are already moving, so stop it
|
||||
// stop();
|
||||
//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;
|
||||
@ -309,7 +312,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.statusPrev = ctrl.status; //update previous status
|
||||
ctrl.status = IDLE;
|
||||
ctrl.stepCnt = 0;
|
||||
gpio_set_level((gpio_num_t)conf.stepPin, 0);
|
||||
@ -344,15 +347,15 @@ bool DendoStepper::xISR()
|
||||
//}
|
||||
|
||||
//CUSTOM: track remaining steps for eventually resuming
|
||||
xSemaphoreTake(semaphore_isrVariables, portMAX_DELAY);
|
||||
ctrl.stepsRemaining = ctrl.stepCnt - ctrl.stepCnt;
|
||||
xSemaphoreGive(semaphore_isrVariables);
|
||||
//xSemaphoreTake(semaphore_isrVariables, portMAX_DELAY);
|
||||
//ctrl.stepsRemaining = ctrl.stepCnt - ctrl.stepCnt;
|
||||
//xSemaphoreGive(semaphore_isrVariables);
|
||||
|
||||
// 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.statusPrev = ctrl.status; //update previous status
|
||||
ctrl.status = IDLE;
|
||||
ctrl.stepCnt = 0;
|
||||
return 0;
|
||||
@ -361,19 +364,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.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.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.statusPrev = ctrl.status; //update previous status
|
||||
ctrl.status = COAST; // we are coasting
|
||||
}
|
||||
|
||||
@ -381,12 +384,14 @@ 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)
|
||||
{
|
||||
//CUSTOM reset counter if already moving
|
||||
ctrl.stepCnt = 0;
|
||||
|
||||
//steps from ctrl.speed -> 0:
|
||||
ctrl.decSteps = 0.5 * ctrl.dec * (ctrl.speed / ctrl.dec) * (ctrl.speed / ctrl.dec);
|
||||
//steps from 0 -> ctrl.speed:
|
||||
|
||||
@ -23,7 +23,7 @@ extern "C"
|
||||
#define STEPPER_TEST_TRAVEL 65 //mm
|
||||
//
|
||||
#define MIN_MM 0
|
||||
#define MAX_MM 60
|
||||
#define MAX_MM 110 //60
|
||||
#define POS_MAX_STEPS MAX_MM * STEPPER_STEPS_PER_MM
|
||||
#define POS_MIN_STEPS MIN_MM * STEPPER_STEPS_PER_MM
|
||||
|
||||
@ -31,10 +31,11 @@ extern "C"
|
||||
#define SPEED_MIN 2.0 //mm/s
|
||||
#define SPEED_MAX 60.0 //mm/s
|
||||
|
||||
#define ACCEL_MS 100.0 //ms from 0 to max
|
||||
#define DECEL_MS 90.0 //ms from max to 0
|
||||
#define SPEED 10 //35, 100, 50 rev
|
||||
#define ACCEL_MS 600.0 //ms from 0 to max
|
||||
#define DECEL_MS 1000.0 //ms from max to 0
|
||||
|
||||
#define STEPPER_STEPS_PER_ROT 800
|
||||
#define STEPPER_STEPS_PER_ROT 1600
|
||||
#define STEPPER_STEPS_PER_MM STEPPER_STEPS_PER_ROT/4
|
||||
|
||||
#define D_CABLE 6
|
||||
@ -252,7 +253,7 @@ void task_stepper_ctl(void *pvParameter)
|
||||
ESP_LOGD(TAG, "cablelen=%.2lf, turns=%.2lf, travelMm=%.3lf, travelStepsExact: %.3lf, travelStepsFull=%d, partialStep=%.3lf", cableLen, turns, travelMm, travelStepsExact, travelStepsFull, travelStepsPartial);
|
||||
ESP_LOGI(TAG, "MOVING %d steps", travelStepsFull);
|
||||
//TODO: calculate variable speed for smoother movement? for example intentionally lag behind and calculate speed according to buffered data
|
||||
step.setSpeedMm(35, 100, 50);
|
||||
step.setSpeedMm(SPEED, ACCEL_MS, DECEL_MS);
|
||||
//testing: get speed from poti
|
||||
//step.setSpeedMm(35, 1000*potiModifier+1, 1000*potiModifier+1);
|
||||
travelSteps(travelStepsExact);
|
||||
|
||||
@ -98,7 +98,7 @@ extern "C" void app_main()
|
||||
xTaskCreate(task_control, "task_control", configMINIMAL_STACK_SIZE * 3, NULL, 5, NULL);
|
||||
|
||||
//create task for controlling the machine
|
||||
xTaskCreate(task_stepper_ctl, "task_stepper_ctl", configMINIMAL_STACK_SIZE * 3, NULL, 5, NULL);
|
||||
xTaskCreate(task_stepper_ctl, "task_stepper_ctl", configMINIMAL_STACK_SIZE * 5, NULL, 5, NULL);
|
||||
|
||||
//create task for handling the buzzer
|
||||
xTaskCreate(&task_buzzer, "task_buzzer", 2048, NULL, 2, NULL);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user