Stepper controller now done

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'},
+  {38, 55, 54, 0, 'x', false, false, true},
-    {56, 61, 60, 0, false, false, 'y'},
+  {56, 61, 60, 0, 'y', false, false, true},
-    {30, 34, 36, 0, false, false, 'e'},
+  {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
+  while (true)
-  free(pam);
+  {
+    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();
+  StepperController::Init(steppers, NUM_STEPPERS);
   
-  // Create tasks
+  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.
+  // The RTOS scheduler manages the tasks.
-
-  // ZCodeParser::ParseString("G0 E20 X40; Comment test\nG1 Y50 X69\nG0 Y235 E5532");
 }

src/Shared/Steppers.h

@@ -1,3 +1,6 @@
+#ifndef STEPPER_H
+#define STEPPER_H
+
 #include <Arduino.h>
 
 struct Stepper {
@@ -5,7 +8,9 @@ struct Stepper {
     uint8_t dirPin;
     uint8_t stepPin;
     int moveQueue;
+    char axis;
     bool steppingEnabled;
     bool motorEnabled;
-    char axis;
+    bool disableOnIdle;
 };
+#endif

src/StepperController.cpp

@@ -1,9 +1,16 @@
 #include "StepperController.h"
 
-int StepperController::getStepperIndex(const char axis){
+// Static variable definitions
-  for (size_t i = 0; i < NumOfSteppers; i++)
+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 (StepperController::Steppers[i].axis == axis)
     {
-    if (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) {
+void StepperController::Init(Stepper *steppers, int numOfSteppers)
-    int index = getStepperIndex(axis);
+{
-    Steppers[index].steppingEnabled = enabled;
+  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);
   }
 
-void StepperController::SetMoveQueue(const char axis, int moveTimeMs) {
+#ifdef ESP32
-    int index = getStepperIndex(axis);
+  // Timer 1 - Equivalent to TIMER1_COMPA_vect
-    Steppers[index].moveQueue = moveTimeMs;
+  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::QueueMove(const char axis, int moveTimeMs) {
+void StepperController::MotorTask(void *params)
-    int index = getStepperIndex(axis);
+{
-    Steppers[index].moveQueue += moveTimeMs; // Fixed incorrect `=+` operator
+  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;
+        }
       }
 
-void StepperController::ClearMoveQueue(const char axis) { // Fixed function name typo
+      // Update motor enable states
-    int index = getStepperIndex(axis);
+      digitalWrite(stepper->enPin, !stepper->motorEnabled);
-    Steppers[index].moveQueue = 0;
     }
 
-void StepperController::SetMotorEnabled(const char axis, bool enabled) {
+    StepperController::lastUpdateTime = millis();
-    int index = getStepperIndex(axis);
+    vTaskDelay(pdMS_TO_TICKS(100));
-    Steppers[index].motorEnabled = enabled;
+  }
+  
+}
+
+void StepperController::SetMovementEnabled(const char axis, bool enabled)
+{
+  int index = getStepperIndex(axis);
+  StepperController::Steppers[index].steppingEnabled = enabled;
+}
+
+void StepperController::SetMoveQueue(const char axis, int moveTimeMs)
+{
+  int index = getStepperIndex(axis);
+  StepperController::Steppers[index].moveQueue = moveTimeMs;
+}
+
+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;
 }

src/StepperController.h

@@ -8,85 +8,30 @@
 #include <Shared/ZCommand.h>
 #include <Shared/MotorMoveParams.h>
 #include <Shared/Steppers.h>
+#include <algorithm>
 
 class StepperController
 {
 private:
-    int NumOfSteppers;
+    static int NumOfSteppers;
-    Stepper *Steppers;
+    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);
+    static void SetMovementEnabled(const char axis, bool enabled);
-    void QueueMove(const char axis, const int moveTimeMs);
-    void ClearMoveQueue(const char axis);
 
-    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