From 3e1418257aef9ad221cdc1a2bc0e7676a26790a2 Mon Sep 17 00:00:00 2001 From: BOTAlex Date: Fri, 24 Jan 2025 17:10:18 +0100 Subject: [PATCH] Stepper controller now done --- src/ClawMachineOverhaul.ino | 38 +++++----- src/Shared/Steppers.h | 9 ++- src/StepperController.cpp | 139 +++++++++++++++++++++++++++++++----- src/StepperController.h | 81 ++++----------------- 4 files changed, 161 insertions(+), 106 deletions(-) diff --git a/src/ClawMachineOverhaul.ino b/src/ClawMachineOverhaul.ino index e491b4b..067b53f 100644 --- a/src/ClawMachineOverhaul.ino +++ b/src/ClawMachineOverhaul.ino @@ -10,9 +10,9 @@ // [X, Y, Head] #define NUM_STEPPERS 3 Stepper steppers[NUM_STEPPERS] = { - {38, 55, 54, 0, false, false, 'x'}, - {56, 61, 60, 0, false, false, 'y'}, - {30, 34, 36, 0, false, false, 'e'}, + {38, 55, 54, 0, 'x', false, false, true}, + {56, 61, 60, 0, 'y', false, false, true}, + {30, 34, 36, 0, 'e', false, false, true}, }; #pragma region Not gonna mess with this magic @@ -51,17 +51,17 @@ void IRAM_ATTR timer3ISR() // Timers for stepping the motors ISR(TIMER3_COMPA_vect) { - if (steppers[0].steppingEnabled) { + if (steppers[0].steppingEnabled && steppers[0].moveQueue != 0) { digitalWrite(steppers[0].stepPin, !digitalRead(steppers[0].stepPin)); } } ISR(TIMER4_COMPA_vect) { - if (steppers[1].steppingEnabled) { + if (steppers[1].steppingEnabled && steppers[1].moveQueue != 0) { digitalWrite(steppers[1].stepPin, !digitalRead(steppers[1].stepPin)); } } ISR(TIMER5_COMPA_vect) { - if (steppers[2].steppingEnabled) { + if (steppers[2].steppingEnabled && steppers[2].moveQueue != 0) { digitalWrite(steppers[2].stepPin, !digitalRead(steppers[2].stepPin)); } } @@ -70,27 +70,29 @@ ISR(TIMER5_COMPA_vect) { #pragma endregion void anotherFunc(void * params){ - MotorMoveParams *pam = (MotorMoveParams *)malloc(sizeof(MotorMoveParams)); - pam->motorChar = 'x'; - pam->moveTimeMs = 3000; - // xTaskCreate(MoveMotor, "Motor", 1024, (void *)pam, 10, NULL); - vTaskDelay(pdMS_TO_TICKS(50)); // Allow time for other task to accept params - free(pam); + while (true) + { + StepperController::SetMotorEnabled('x', true); + StepperController::SetMovementEnabled('x', true); + StepperController::QueueMove('x', 2000); + Serial.println("Added to queue"); + vTaskDelay(pdMS_TO_TICKS(5000)); + } + + vTaskDelete(NULL); } void setup() { Serial.begin(115200); - // setupMotor(); - - // Create tasks + StepperController::Init(steppers, NUM_STEPPERS); + + xTaskCreate(StepperController::MotorTask, "Motors", 1024, NULL, 5, NULL); xTaskCreate(anotherFunc, "Other loop", 1024, NULL, 5, NULL); } void loop() { - // The RTOS scheduler manages the tasks. No code is needed here. - - // ZCodeParser::ParseString("G0 E20 X40; Comment test\nG1 Y50 X69\nG0 Y235 E5532"); + // The RTOS scheduler manages the tasks. } diff --git a/src/Shared/Steppers.h b/src/Shared/Steppers.h index 6d05718..9d73928 100644 --- a/src/Shared/Steppers.h +++ b/src/Shared/Steppers.h @@ -1,3 +1,6 @@ +#ifndef STEPPER_H +#define STEPPER_H + #include struct Stepper { @@ -5,7 +8,9 @@ struct Stepper { uint8_t dirPin; uint8_t stepPin; int moveQueue; + char axis; bool steppingEnabled; bool motorEnabled; - char axis; -}; \ No newline at end of file + bool disableOnIdle; +}; +#endif \ No newline at end of file diff --git a/src/StepperController.cpp b/src/StepperController.cpp index 19a2bfe..8c1fc90 100644 --- a/src/StepperController.cpp +++ b/src/StepperController.cpp @@ -1,9 +1,16 @@ #include "StepperController.h" -int StepperController::getStepperIndex(const char axis){ - for (size_t i = 0; i < NumOfSteppers; i++) +// Static variable definitions +int StepperController::NumOfSteppers = 0; +Stepper *StepperController::Steppers = nullptr; +long StepperController::lastUpdateTime = 0; + +int StepperController::getStepperIndex(const char axis) +{ + for (size_t i = 0; i < StepperController::NumOfSteppers; i++) { - if (Steppers[i].axis == axis) { + if (StepperController::Steppers[i].axis == axis) + { // Serial.print("Found motor: "); // Serial.println(i); return i; @@ -12,27 +19,123 @@ int StepperController::getStepperIndex(const char axis){ return -1; } -void StepperController::SetMovementEnabled(const char axis, bool enabled) { - int index = getStepperIndex(axis); - Steppers[index].steppingEnabled = enabled; +void StepperController::Init(Stepper *steppers, int numOfSteppers) +{ + StepperController::Steppers = steppers; + StepperController::NumOfSteppers = numOfSteppers; + StepperController::lastUpdateTime = 0; + + for (size_t i = 0; i < NumOfSteppers; i++) + { + pinMode(steppers[i].enPin, OUTPUT); + pinMode(steppers[i].dirPin, OUTPUT); + pinMode(steppers[i].stepPin, OUTPUT); + } + +#ifdef ESP32 + // Timer 1 - Equivalent to TIMER1_COMPA_vect + timer1 = timerBegin(0, 80, true); // Timer 0, prescaler 80 (1us per tick) + timerAttachInterrupt(timer1, &timer1ISR, true); + timerAlarmWrite(timer1, 500, true); // 500us interval + timerAlarmEnable(timer1); + + // Timer 2 - Equivalent to TIMER3_COMPA_vect + timer2 = timerBegin(1, 80, true); // Timer 1, prescaler 80 (1us per tick) + timerAttachInterrupt(timer2, &timer2ISR, true); + timerAlarmWrite(timer2, 500, true); // 500us interval + timerAlarmEnable(timer2); + + // Timer 3 - Equivalent to TIMER4_COMPA_vect + timer3 = timerBegin(2, 80, true); // Timer 2, prescaler 80 (1us per tick) + timerAttachInterrupt(timer3, &timer3ISR, true); + timerAlarmWrite(timer3, 500, true); // 500us interval + timerAlarmEnable(timer3); +#else + cli(); + // Timer 3 (16-bit) + TCCR3A = 0; + TCCR3B = (1 << WGM32) | (1 << CS30); // CTC mode, no prescaler + OCR3A = 499; // Compare match value + TIMSK3 |= (1 << OCIE3A); // Enable Timer1 Compare A Match Interrupt + + // Timer 4 (16-bit) + TCCR4A = 0; + TCCR4B = (1 << WGM42) | (1 << CS40); // CTC mode, no prescaler + OCR4A = 499; // Compare match value + TIMSK4 |= (1 << OCIE4A); // Enable Timer3 Compare A Match Interrupt + + // Timer 5 (16-bit) + TCCR5A = 0; + TCCR5B = (1 << WGM52) | (1 << CS50); // CTC mode, no prescaler + OCR5A = 499; // Compare match value + TIMSK5 |= (1 << OCIE5A); // Enable Timer1 Compare A Match Interrupt + sei(); +#endif } -void StepperController::SetMoveQueue(const char axis, int moveTimeMs) { - int index = getStepperIndex(axis); - Steppers[index].moveQueue = moveTimeMs; +void StepperController::MotorTask(void *params) +{ + while (true) + { + // Serial.println("Motor loop"); + long sinceLastRunTime = millis() - StepperController::lastUpdateTime; + + for (size_t i = 0; i < (size_t)StepperController::NumOfSteppers; i++) + { + Stepper *stepper = &StepperController:: Steppers[i]; + bool isNegative = signbit(stepper->moveQueue); + + if (stepper->steppingEnabled && stepper->moveQueue != 0) + { + if (stepper->moveQueue > 0) { + stepper->moveQueue -= std::min((int)sinceLastRunTime, abs(stepper->moveQueue)); + } else { + stepper->moveQueue += std::min((int)sinceLastRunTime, abs(stepper->moveQueue)); + } + Serial.println(stepper->moveQueue); + digitalWrite(stepper->dirPin, isNegative); + + if (stepper->disableOnIdle && stepper->moveQueue == 0){ + stepper->motorEnabled = false; + } + } + + // Update motor enable states + digitalWrite(stepper->enPin, !stepper->motorEnabled); + } + + StepperController::lastUpdateTime = millis(); + vTaskDelay(pdMS_TO_TICKS(100)); + } + } -void StepperController::QueueMove(const char axis, int moveTimeMs) { - int index = getStepperIndex(axis); - Steppers[index].moveQueue += moveTimeMs; // Fixed incorrect `=+` operator +void StepperController::SetMovementEnabled(const char axis, bool enabled) +{ + int index = getStepperIndex(axis); + StepperController::Steppers[index].steppingEnabled = enabled; } -void StepperController::ClearMoveQueue(const char axis) { // Fixed function name typo - int index = getStepperIndex(axis); - Steppers[index].moveQueue = 0; +void StepperController::SetMoveQueue(const char axis, int moveTimeMs) +{ + int index = getStepperIndex(axis); + StepperController::Steppers[index].moveQueue = moveTimeMs; } -void StepperController::SetMotorEnabled(const char axis, bool enabled) { - int index = getStepperIndex(axis); - Steppers[index].motorEnabled = enabled; +void StepperController::QueueMove(const char axis, int moveTimeMs) +{ + int index = getStepperIndex(axis); + StepperController::Steppers[index].moveQueue += moveTimeMs; +} + +void StepperController::ClearMoveQueue(const char axis) +{ + int index = getStepperIndex(axis); + StepperController::Steppers[index].moveQueue = 0; +} + +void StepperController::SetMotorEnabled(const char axis, bool enabled) +{ + int index = getStepperIndex(axis); + StepperController::Steppers[index].motorEnabled = enabled; } diff --git a/src/StepperController.h b/src/StepperController.h index 3415eca..f21e59b 100644 --- a/src/StepperController.h +++ b/src/StepperController.h @@ -8,85 +8,30 @@ #include #include #include +#include class StepperController { private: - int NumOfSteppers; - Stepper *Steppers; + static int NumOfSteppers; + static Stepper *Steppers; + static long lastUpdateTime; + + static int getStepperIndex(const char axis); - int getStepperIndex(const char axis); public: - StepperController(Stepper *steppers, int numOfSteppers = 3); + static void Init(Stepper *steppers, int numOfSteppers = 3); - void SetMovementEnabled(const char axis, bool enabled); + static void MotorTask(void * params); - void SetMoveQueue(const char axis, const int moveTimeMs); - void QueueMove(const char axis, const int moveTimeMs); - void ClearMoveQueue(const char axis); + static void SetMovementEnabled(const char axis, bool enabled); - void SetMotorEnabled(const char axis, bool enabled); + static void SetMoveQueue(const char axis, const int moveTimeMs); + static void QueueMove(const char axis, const int moveTimeMs); + static void ClearMoveQueue(const char axis); - ~StepperController(); + static void SetMotorEnabled(const char axis, bool enabled); }; -StepperController::StepperController(Stepper steppers[], int numOfSteppers = 3) -{ - Steppers = steppers; - NumOfSteppers = numOfSteppers; - - for (size_t i = 0; i < NumOfSteppers; i++) - { - pinMode(steppers[i].enPin, OUTPUT); - pinMode(steppers[i].dirPin, OUTPUT); - pinMode(steppers[i].stepPin, OUTPUT); - } - -#ifdef ESP32 - // Timer 1 - Equivalent to TIMER1_COMPA_vect - timer1 = timerBegin(0, 80, true); // Timer 0, prescaler 80 (1us per tick) - timerAttachInterrupt(timer1, &timer1ISR, true); - timerAlarmWrite(timer1, 500, true); // 500us interval - timerAlarmEnable(timer1); - - // Timer 2 - Equivalent to TIMER3_COMPA_vect - timer2 = timerBegin(1, 80, true); // Timer 1, prescaler 80 (1us per tick) - timerAttachInterrupt(timer2, &timer2ISR, true); - timerAlarmWrite(timer2, 500, true); // 500us interval - timerAlarmEnable(timer2); - - // Timer 3 - Equivalent to TIMER4_COMPA_vect - timer3 = timerBegin(2, 80, true); // Timer 2, prescaler 80 (1us per tick) - timerAttachInterrupt(timer3, &timer3ISR, true); - timerAlarmWrite(timer3, 500, true); // 500us interval - timerAlarmEnable(timer3); - #else - cli(); - // Timer 3 (16-bit) - TCCR3A = 0; - TCCR3B = (1 << WGM32) | (1 << CS30); // CTC mode, no prescaler - OCR3A = 499; // Compare match value - TIMSK3 |= (1 << OCIE3A); // Enable Timer1 Compare A Match Interrupt - - // Timer 4 (16-bit) - TCCR4A = 0; - TCCR4B = (1 << WGM42) | (1 << CS40); // CTC mode, no prescaler - OCR4A = 499; // Compare match value - TIMSK4 |= (1 << OCIE4A); // Enable Timer3 Compare A Match Interrupt - - // Timer 5 (16-bit) - TCCR5A = 0; - TCCR5B = (1 << WGM42) | (1 << CS50); // CTC mode, no prescaler - OCR5A = 499; // Compare match value - TIMSK5 |= (1 << OCIE5A); // Enable Timer1 Compare A Match Interrupt - sei(); - #endif -} - -StepperController::~StepperController() -{ - // free(Steppers); // Meh, let's go for a memory leak. ez -} - #endif \ No newline at end of file