Simple version now working just as good as that complex

src/MotorControlPart.h

@@ -10,18 +10,27 @@
 #define JOYSTICK_PIN 63
 
 struct CharMapping {
-    char character;
+  char character;
-    int value;
+  int value;
 };
 
-// [X, Y, Head] TODO: 
+struct Stepper {
-#define NUM_MOTORS 3
+  uint8_t enPin;
-static const uint8_t STEP_PINS[NUM_MOTORS] = {54, 60, 36};
+  uint8_t dirPin;
-static const uint8_t DIR_PINS[NUM_MOTORS] = {55, 61, 34};
+  uint8_t stepPin;
-static const uint8_t EN_PINS[NUM_MOTORS] = {38, 56, 30};
+  bool timerEnabled;
-bool motorsEnabled[NUM_MOTORS] = {false, false, false};
+  bool motorEnabled;
-CharMapping mappings[] = {{'x', 0}, {'y', 1}, {'e', 2}};
+  char axis;
-uint8_t selectedIndex = 1;
+};
+
+
+// [X, Y, Head]
+#define NUM_STEPPERS 3
+Stepper steppers[NUM_STEPPERS] = {
+  {38, 55, 54, false, false, 'x'},
+  {56, 61, 60, false, false, 'y'},
+  {30, 34, 36, false, false, 'e'},
+};
 
 // Critical range for detecting joystick movement
 static const int JOY_CRITICAL_POINT = 25;
@@ -63,108 +72,72 @@ void IRAM_ATTR timer3ISR()
 
 // Timers for stepping the motors
 ISR(TIMER3_COMPA_vect) {
-  if (motorsEnabled[0]) {
+  if (steppers[0].timerEnabled) {
-    digitalWrite(STEP_PINS[0], !digitalRead(STEP_PINS[0]));
+    digitalWrite(steppers[0].stepPin, !digitalRead(steppers[0].stepPin));
   }
 }
 ISR(TIMER4_COMPA_vect) {
-  if (motorsEnabled[1]) {
+  if (steppers[1].timerEnabled) {
-    digitalWrite(STEP_PINS[1], !digitalRead(STEP_PINS[1]));
+    digitalWrite(steppers[1].stepPin, !digitalRead(steppers[1].stepPin));
   }
 }
 ISR(TIMER5_COMPA_vect) {
-  if (motorsEnabled[2]) {
+  if (steppers[2].timerEnabled) {
-    digitalWrite(STEP_PINS[2], !digitalRead(STEP_PINS[2]));
+    digitalWrite(steppers[2].stepPin, !digitalRead(steppers[2].stepPin));
   }
 }
 #endif
 
-int charToMotorIndex(char c) {
-    for (int i = 0; i < NUM_MOTORS; i++) {
-        if (mappings[i].character == c) {
-            return mappings[i].value;
-        }
-    }
-    return -1; // Not found
-}
-
-void updateInput() {
-  int joystickValueRaw = analogRead(JOYSTICK_PIN);
-  int joystickValue = map(joystickValueRaw, 0, 1023, 0, 100);
-  bool leftMovement = (joystickValue > (100 - JOY_CRITICAL_POINT));
-  bool rightMovement = (joystickValue < JOY_CRITICAL_POINT);
-  motorsEnabled[selectedIndex] = leftMovement || rightMovement;
-
-  digitalWrite(EN_PINS[selectedIndex], !motorsEnabled[selectedIndex]); // Inverted for some reason
-  digitalWrite(DIR_PINS[selectedIndex], leftMovement);
-
-  // // Debug
-  // Serial.print("Raw: "); Serial.print(joystickValueRaw);
-  // Serial.print(" | Mapped: "); Serial.print(joystickValue);
-  // Serial.print(" | Left: "); Serial.print(leftMovement);
-  // Serial.print(" | Right: "); Serial.print(rightMovement);
-  // Serial.print(" | Motor: "); Serial.println(motorsEnabled[selectedIndex] ? "ON" : "OFF");
-}
-
 void ExecuteCommand(void *params){
   ZCommand receivedValue = *((ZCommand *)params);
 }
 
+int getStepperIndex(const char axis){
+  for (size_t i = 0; i < NUM_STEPPERS; i++)
+  {
+    if (steppers[i].axis == axis) {
+      Serial.print("Found motor: ");
+      Serial.println(i);
+      return i;
+    }
+  }
+  return -1;
+}
+
 void MoveMotor(void *params) {
   MotorMoveParams *motorParams = ((MotorMoveParams *) params);
   char motorChar = motorParams->motorChar;
   float moveTimeMs = motorParams->moveTimeMs;
-
-  int index = charToMotorIndex(motorChar);
   int isBackwards = signbit(moveTimeMs); // Negative means move backwards, positive means move forward.
 
+  int index = getStepperIndex(motorChar);
+  Stepper *currentStepper = &steppers[index];
+
   // Enable motor and set direction
-  motorsEnabled[index] = true;
+  Serial.println("Start motor.");
-  digitalWrite(EN_PINS[index], !motorsEnabled[index]); // Inverted for some reason
+  currentStepper->motorEnabled = true;
-  digitalWrite(DIR_PINS[index], isBackwards);
+  currentStepper->timerEnabled = true;
+  digitalWrite(currentStepper->enPin, !(currentStepper->motorEnabled)); // Inverted for some reason
+  digitalWrite(currentStepper->dirPin, isBackwards);
   
   vTaskDelay(pdMS_TO_TICKS(moveTimeMs));
 
   // Stop motor
-  motorsEnabled[index] = false;
+  Serial.println("Stop motor.");
-  digitalWrite(EN_PINS[index], !motorsEnabled[index]); // Inverted for some reason
+  currentStepper->motorEnabled = false;
+  currentStepper->timerEnabled = false;
+  digitalWrite(currentStepper->enPin, !(currentStepper->motorEnabled)); // Inverted for some reason
   vTaskDelete(NULL);
 }
 
 
-void MotorControlTask(void *params) {
-  while (true) {
-    updateInput();
-    
-    // Check if there is serial data available
-    while (Serial.available() > 0) {
-      String input = Serial.readStringUntil('\n'); // Read input until newline
-      input.trim(); // Remove any whitespace
-
-      // Check if the input matches the expected format ":0", ":1", or ":2"
-      if (input.length() == 2 && input[0] == ':') {
-        char indexChar = input[1];
-        if (indexChar >= '0' && indexChar <= '2') {
-          selectedIndex = indexChar - '0'; // Convert character to integer
-          Serial.print("Selected index set to: ");
-          Serial.println(selectedIndex);
-        } else {
-          Serial.println("Invalid index! Use :0, :1, or :2");
-        }
-      }
-    }
-
-    vTaskDelay(pdMS_TO_TICKS(50)); // Poll every 50ms
-  }
-}
-
 // Configures Timer1 for compare match interrupts
 void setupMotor() {
-    for (size_t i = 0; i < NUM_MOTORS; i++)
+    for (size_t i = 0; i < NUM_STEPPERS; i++)
     {
-        pinMode(STEP_PINS[i], OUTPUT);
+        pinMode(steppers[i].enPin, OUTPUT);
-        pinMode(DIR_PINS[i], OUTPUT);
+        pinMode(steppers[i].dirPin, OUTPUT);
-        pinMode(EN_PINS[i], OUTPUT);
+        pinMode(steppers[i].stepPin, OUTPUT);
     }
     
 #ifdef ESP32