New implementation with timer, partial functional (WIP)
Partially implemented stepper driver as planned in diagram works partially (reset button toggles distances) major issues though - wip needs debugging and testing
This commit is contained in:
jonny_ji7
parent
b6a7ee65ed
commit
61deaf9ead
File diff suppressed because one or more lines are too long
@ -168,6 +168,7 @@ void updateSpeedFromAdc() {
|
||||
void task_stepper_test(void *pvParameter)
|
||||
{
|
||||
stepper_init();
|
||||
int state = 0;
|
||||
while(1){
|
||||
vTaskDelay(20 / portTICK_PERIOD_MS);
|
||||
|
||||
@ -183,21 +184,28 @@ void task_stepper_test(void *pvParameter)
|
||||
SW_AUTO_CUT.handle();
|
||||
|
||||
if (SW_RESET.risingEdge) {
|
||||
stepper_toggleDirection();
|
||||
buzzer.beep(1, 1000, 100);
|
||||
}
|
||||
if (SW_PRESET1.risingEdge) {
|
||||
buzzer.beep(2, 300, 100);
|
||||
stepperSw_setTargetSteps(1000);
|
||||
}
|
||||
if (SW_PRESET2.risingEdge) {
|
||||
buzzer.beep(1, 500, 100);
|
||||
stepperSw_setTargetSteps(10000);
|
||||
}
|
||||
if (SW_PRESET3.risingEdge) {
|
||||
buzzer.beep(1, 100, 100);
|
||||
stepperSw_setTargetSteps(30000);
|
||||
//stepper_toggleDirection();
|
||||
//buzzer.beep(1, 1000, 100);
|
||||
if (state) {
|
||||
stepper_setTargetSteps(1000);
|
||||
state = 0;
|
||||
} else {
|
||||
stepper_setTargetSteps(600);
|
||||
state = 1;
|
||||
}
|
||||
}
|
||||
// if (SW_PRESET1.risingEdge) {
|
||||
// buzzer.beep(2, 300, 100);
|
||||
// stepperSw_setTargetSteps(1000);
|
||||
// }
|
||||
// if (SW_PRESET2.risingEdge) {
|
||||
// buzzer.beep(1, 500, 100);
|
||||
// stepperSw_setTargetSteps(10000);
|
||||
// }
|
||||
// if (SW_PRESET3.risingEdge) {
|
||||
// buzzer.beep(1, 100, 100);
|
||||
// stepperSw_setTargetSteps(30000);
|
||||
// }
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -94,7 +94,7 @@ extern "C" void app_main()
|
||||
|
||||
#ifdef STEPPER_TEST
|
||||
//create task for stepper testing
|
||||
xTaskCreate(task_stepperCtrlSw, "task stepper control software", configMINIMAL_STACK_SIZE * 3, NULL, configMAX_PRIORITIES - 1, NULL);
|
||||
//xTaskCreate(task_stepperCtrlSw, "task stepper control software", configMINIMAL_STACK_SIZE * 3, NULL, configMAX_PRIORITIES - 1, NULL);
|
||||
xTaskCreate(task_stepper_test, "task_stepper_test", configMINIMAL_STACK_SIZE * 3, NULL, 2, NULL);
|
||||
#else
|
||||
//create task for controlling the machine
|
||||
|
||||
215
main/stepper.cpp
215
main/stepper.cpp
@ -1,7 +1,9 @@
|
||||
//custom driver for stepper motor
|
||||
|
||||
#include "config.hpp"
|
||||
#include "hal/timer_types.h"
|
||||
|
||||
#include <cstdint>
|
||||
#include <inttypes.h>
|
||||
|
||||
//config from config.hpp
|
||||
@ -20,13 +22,16 @@ extern "C" {
|
||||
#define NS_TO_T_TICKS(x) (x)
|
||||
|
||||
static const char *TAG = "stepper-ctl"; //tag for logging
|
||||
bool direction = 0;
|
||||
bool direction = 1;
|
||||
bool timerIsRunning = false;
|
||||
|
||||
bool timer_isr(void *arg);
|
||||
|
||||
|
||||
|
||||
static timer_group_t timerGroup = TIMER_GROUP_0;
|
||||
static timer_idx_t timerIdx = TIMER_0;
|
||||
static uint64_t posTarget = 0;
|
||||
static uint64_t posNow = 0;
|
||||
|
||||
|
||||
//stepper driver in software for testing (no timer/interrupt):
|
||||
@ -101,26 +106,23 @@ StepperControl ctrl; // Create an instance of StepperControl struct
|
||||
|
||||
|
||||
void stepper_setTargetSteps(int target_steps) {
|
||||
ESP_LOGW(TAG, "set target steps to %d", target_steps);
|
||||
ESP_LOGW(TAG, "set target steps from %lld to %d (stepsNow: %llu", (long long int)posTarget, target_steps, (long long int)posNow);
|
||||
posTarget = target_steps;
|
||||
//TODO switch dir pin in isr? not in sync with count
|
||||
//TODO switch direction using negative values as below
|
||||
// if(target_steps < 0){
|
||||
// gpio_set_level(ctrl.directionPin, 1);
|
||||
// } else {
|
||||
// gpio_set_level(ctrl.directionPin, 0);
|
||||
// }
|
||||
ESP_LOGW(TAG, "toggle direction -> %d", direction);
|
||||
|
||||
|
||||
// Update the targetSteps value
|
||||
ctrl.targetSteps = abs(target_steps);
|
||||
//ctrl.targetSteps = abs(target_steps);
|
||||
|
||||
// Check if the timer is currently paused
|
||||
ESP_LOGW(TAG, "check if timer is running %d", timerIsRunning);
|
||||
if (!timerIsRunning){
|
||||
// If the timer is paused, start it again with the updated targetSteps
|
||||
timer_set_alarm_value(ctrl.timerGroup, ctrl.timerIdx, 1000);
|
||||
timer_set_counter_value(ctrl.timerGroup, ctrl.timerIdx, 1000);
|
||||
timer_start(ctrl.timerGroup, ctrl.timerIdx);
|
||||
timerIsRunning = true;
|
||||
ESP_LOGW(TAG, "starting timer because did not run before");
|
||||
ESP_ERROR_CHECK(timer_set_alarm_value(timerGroup, timerIdx, 1000));
|
||||
//timer_set_counter_value(timerGroup, timerIdx, 1000);
|
||||
ESP_ERROR_CHECK(timer_start(timerGroup, timerIdx));
|
||||
ESP_LOGW(TAG, "STARTED TIMER");
|
||||
}
|
||||
}
|
||||
@ -183,56 +185,157 @@ void stepper_init(){
|
||||
|
||||
|
||||
bool timer_isr(void *arg) {
|
||||
// Cast arg to an integer type that has the same size as timer_idx_t
|
||||
uintptr_t arg_val = (uintptr_t)arg;
|
||||
// Cast arg_val to timer_idx_t
|
||||
timer_idx_t timer_idx = (timer_idx_t)arg_val;
|
||||
int32_t step_diff = ctrl.targetSteps - ctrl.currentSteps;
|
||||
timerIsRunning = true;
|
||||
//turn pin on (fast)
|
||||
GPIO.out_w1ts = (1ULL << STEPPER_STEP_PIN);
|
||||
|
||||
if (timer_idx == ctrl.timerIdx) {
|
||||
if (ctrl.currentSteps < ctrl.targetSteps) {
|
||||
// Check if accelerating
|
||||
if (ctrl.isAccelerating) {
|
||||
if (ctrl.currentSpeed < ctrl.targetSpeed) {
|
||||
// Increase speed if not yet at target speed
|
||||
ctrl.currentSpeed += ctrl.acceleration;
|
||||
} else {
|
||||
// Reached target speed, clear accelerating flag
|
||||
ctrl.isAccelerating = false;
|
||||
}
|
||||
}
|
||||
//FIXME controller crashes when finished accelerating
|
||||
//Guru Meditation Error: Core 0 panic'ed (Interrupt wdt timeout on CPU0).
|
||||
|
||||
// Check if decelerating
|
||||
if (ctrl.isDecelerating) { //FIXME isDecelerating is never set???
|
||||
if (ctrl.currentSpeed > ctrl.targetSpeed) {
|
||||
// Decrease speed if not yet at target speed
|
||||
ctrl.currentSpeed -= ctrl.deceleration;
|
||||
} else {
|
||||
// Reached target speed, clear decelerating flag
|
||||
ctrl.isDecelerating = false;
|
||||
}
|
||||
}
|
||||
uint32_t speedTarget = 100000;
|
||||
uint32_t speedMin = 20000;
|
||||
//FIXME increment actually has to be re-calculated every run to have linear accel (because also gets called faster/slower)
|
||||
uint32_t decel_increment = 200;
|
||||
uint32_t accel_increment = 150;
|
||||
|
||||
// Generate pulse for stepper motor
|
||||
gpio_set_level(ctrl.pulsePin, 1);
|
||||
ets_delay_us(500); // Adjust delay as needed
|
||||
gpio_set_level(ctrl.pulsePin, 0);
|
||||
static uint64_t stepsToGo = 0;
|
||||
static uint32_t speedNow = speedMin;
|
||||
|
||||
// Update current step count
|
||||
ctrl.currentSteps++;
|
||||
|
||||
// Update timer period based on current speed
|
||||
timer_set_alarm_value(ctrl.timerGroup, ctrl.timerIdx, TIMER_BASE_CLK / ctrl.currentSpeed);
|
||||
//--- define direction, stepsToGo ---
|
||||
//int64_t delta = (int)posTarget - (int)posNow;
|
||||
//bool directionTarget = delta >= 0 ? 1 : 0;
|
||||
bool directionTarget;
|
||||
if (posTarget > posNow) {
|
||||
directionTarget = 1;
|
||||
} else {
|
||||
directionTarget = 0;
|
||||
}
|
||||
|
||||
//directionTarget = 1;
|
||||
//direction = 1;
|
||||
//gpio_set_level(STEPPER_DIR_PIN, direction);
|
||||
|
||||
if (direction != directionTarget) {
|
||||
//ESP_LOGW(TAG, "direction differs! new: %d", direction);
|
||||
if (stepsToGo == 0){
|
||||
direction = directionTarget; //switch direction if almost idle
|
||||
gpio_set_level(STEPPER_DIR_PIN, direction);
|
||||
//stepsToGo = abs((int64_t)posTarget - (int64_t)posNow);
|
||||
stepsToGo = 2;
|
||||
} else {
|
||||
// Reached target step count, stop timer
|
||||
timer_pause(ctrl.timerGroup, ctrl.timerIdx);
|
||||
timerIsRunning = false;
|
||||
//ESP_LOGW(TAG,"finished, pausing timer");
|
||||
//stepsToGo = speedNow / decel_increment; //set steps to decel to min speed
|
||||
stepsToGo = speedNow / decel_increment;
|
||||
}
|
||||
} else if (direction == 1) {
|
||||
stepsToGo = posTarget - posNow;
|
||||
//stepsToGo = abs((int64_t)posTarget - (int64_t)posNow);
|
||||
} else {
|
||||
stepsToGo = posNow - posTarget;
|
||||
//stepsToGo = abs((int64_t)posTarget - (int64_t)posNow);
|
||||
}
|
||||
|
||||
//TODO fix direction code above currently ony works with the below line instead
|
||||
//stepsToGo = abs((int64_t)posTarget - (int64_t)posNow);
|
||||
|
||||
|
||||
//--- define speed ---
|
||||
//uint64_t stepsAccelRemaining = (speedTarget - speedNow) / accel_increment;
|
||||
uint64_t stepsDecelRemaining = (speedNow - speedMin) / decel_increment;
|
||||
|
||||
if (stepsToGo <= stepsDecelRemaining) {
|
||||
if ((speedNow - speedMin) > decel_increment) {
|
||||
speedNow -= decel_increment;
|
||||
} else {
|
||||
speedNow = speedMin; //PAUSE HERE??? / irrelevant?
|
||||
}
|
||||
}
|
||||
else if (speedNow < speedTarget) {
|
||||
speedNow += accel_increment;
|
||||
if (speedNow > speedTarget) speedNow = speedTarget;
|
||||
}
|
||||
else { //not relevant?
|
||||
speedNow = speedTarget;
|
||||
}
|
||||
|
||||
|
||||
//--- update timer ---
|
||||
if (stepsToGo == 0) {
|
||||
timer_pause(timerGroup, timerIdx);
|
||||
timerIsRunning = false;
|
||||
}
|
||||
else {
|
||||
ESP_ERROR_CHECK(timer_set_alarm_value(timerGroup, timerIdx, TIMER_BASE_CLK / speedNow));
|
||||
}
|
||||
|
||||
|
||||
//--- increment position ---
|
||||
if (stepsToGo > 0){
|
||||
stepsToGo --; //TODO increment at start, check at start??
|
||||
}
|
||||
if (direction == 1){
|
||||
posNow ++;
|
||||
} else {
|
||||
posNow --; //FIXME posNow gets extremely big after second start (underflow?)
|
||||
}
|
||||
|
||||
|
||||
// Generate pulse for stepper motor
|
||||
//gpio_set_level(STEPPER_STEP_PIN, 1);
|
||||
//ets_delay_us(500); // Adjust delay as needed
|
||||
//gpio_set_level(STEPPER_STEP_PIN, 0);
|
||||
|
||||
//turn pin off (fast)
|
||||
GPIO.out_w1tc = (1ULL << STEPPER_STEP_PIN);
|
||||
|
||||
|
||||
// // Cast arg to an integer type that has the same size as timer_idx_t
|
||||
// uintptr_t arg_val = (uintptr_t)arg;
|
||||
// // Cast arg_val to timer_idx_t
|
||||
// timer_idx_t timer_idx = (timer_idx_t)arg_val;
|
||||
// int32_t step_diff = ctrl.targetSteps - ctrl.currentSteps;
|
||||
// timerIsRunning = true;
|
||||
//
|
||||
// if (timer_idx == ctrl.timerIdx) {
|
||||
// if (ctrl.currentSteps < ctrl.targetSteps) {
|
||||
// // Check if accelerating
|
||||
// if (ctrl.isAccelerating) {
|
||||
// if (ctrl.currentSpeed < ctrl.targetSpeed) {
|
||||
// // Increase speed if not yet at target speed
|
||||
// ctrl.currentSpeed += ctrl.acceleration;
|
||||
// } else {
|
||||
// // Reached target speed, clear accelerating flag
|
||||
// ctrl.isAccelerating = false;
|
||||
// }
|
||||
// }
|
||||
// //FIXME controller crashes when finished accelerating
|
||||
// //Guru Meditation Error: Core 0 panic'ed (Interrupt wdt timeout on CPU0).
|
||||
//
|
||||
// // Check if decelerating
|
||||
// if (ctrl.isDecelerating) { //FIXME isDecelerating is never set???
|
||||
// if (ctrl.currentSpeed > ctrl.targetSpeed) {
|
||||
// // Decrease speed if not yet at target speed
|
||||
// ctrl.currentSpeed -= ctrl.deceleration;
|
||||
// } else {
|
||||
// // Reached target speed, clear decelerating flag
|
||||
// ctrl.isDecelerating = false;
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// // Generate pulse for stepper motor
|
||||
// gpio_set_level(ctrl.pulsePin, 1);
|
||||
// ets_delay_us(500); // Adjust delay as needed
|
||||
// gpio_set_level(ctrl.pulsePin, 0);
|
||||
//
|
||||
// // Update current step count
|
||||
// ctrl.currentSteps++;
|
||||
//
|
||||
// // Update timer period based on current speed
|
||||
// timer_set_alarm_value(ctrl.timerGroup, ctrl.timerIdx, TIMER_BASE_CLK / ctrl.currentSpeed);
|
||||
// } else {
|
||||
// // Reached target step count, stop timer
|
||||
// timer_pause(ctrl.timerGroup, ctrl.timerIdx);
|
||||
// timerIsRunning = false;
|
||||
// //ESP_LOGW(TAG,"finished, pausing timer");
|
||||
// }
|
||||
// }
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user