From a9d468d87c0779726c9b81bff981dc83d939ee08 Mon Sep 17 00:00:00 2001 From: BOTAlex Date: Thu, 23 Jan 2025 04:25:53 +0100 Subject: [PATCH] Simple version now working just as good as that complex --- src/MotorControlPart.h | 133 ++++++++++++++++------------------------- 1 file changed, 53 insertions(+), 80 deletions(-) diff --git a/src/MotorControlPart.h b/src/MotorControlPart.h index b18537e..0fd34a6 100644 --- a/src/MotorControlPart.h +++ b/src/MotorControlPart.h @@ -10,18 +10,27 @@ #define JOYSTICK_PIN 63 struct CharMapping { - char character; - int value; + char character; + int value; }; -// [X, Y, Head] TODO: -#define NUM_MOTORS 3 -static const uint8_t STEP_PINS[NUM_MOTORS] = {54, 60, 36}; -static const uint8_t DIR_PINS[NUM_MOTORS] = {55, 61, 34}; -static const uint8_t EN_PINS[NUM_MOTORS] = {38, 56, 30}; -bool motorsEnabled[NUM_MOTORS] = {false, false, false}; -CharMapping mappings[] = {{'x', 0}, {'y', 1}, {'e', 2}}; -uint8_t selectedIndex = 1; +struct Stepper { + uint8_t enPin; + uint8_t dirPin; + uint8_t stepPin; + bool timerEnabled; + bool motorEnabled; + char axis; +}; + + +// [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]) { - digitalWrite(STEP_PINS[0], !digitalRead(STEP_PINS[0])); + if (steppers[0].timerEnabled) { + digitalWrite(steppers[0].stepPin, !digitalRead(steppers[0].stepPin)); } } ISR(TIMER4_COMPA_vect) { - if (motorsEnabled[1]) { - digitalWrite(STEP_PINS[1], !digitalRead(STEP_PINS[1])); + if (steppers[1].timerEnabled) { + digitalWrite(steppers[1].stepPin, !digitalRead(steppers[1].stepPin)); } } ISR(TIMER5_COMPA_vect) { - if (motorsEnabled[2]) { - digitalWrite(STEP_PINS[2], !digitalRead(STEP_PINS[2])); + if (steppers[2].timerEnabled) { + 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; - digitalWrite(EN_PINS[index], !motorsEnabled[index]); // Inverted for some reason - digitalWrite(DIR_PINS[index], isBackwards); + Serial.println("Start motor."); + currentStepper->motorEnabled = true; + 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; - digitalWrite(EN_PINS[index], !motorsEnabled[index]); // Inverted for some reason + Serial.println("Stop motor."); + 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(DIR_PINS[i], OUTPUT); - pinMode(EN_PINS[i], OUTPUT); + pinMode(steppers[i].enPin, OUTPUT); + pinMode(steppers[i].dirPin, OUTPUT); + pinMode(steppers[i].stepPin, OUTPUT); } #ifdef ESP32