diff --git a/src/ClawMachineOverhaul.ino b/src/ClawMachineOverhaul.ino index c5e34fc..780120e 100644 --- a/src/ClawMachineOverhaul.ino +++ b/src/ClawMachineOverhaul.ino @@ -1,98 +1,6 @@ #include #include - -// Pin used for the joystick -#define JOYSTICK_PIN 63 - -// Motor pins -static const uint8_t STEP_PIN = 54; -static const uint8_t DIR_PIN = 55; -static const uint8_t EN_PIN = 38; - -// Critical range for detecting joystick movement -static const int JOY_CRITICAL_POINT = 20; - -// Tracking joystick directions -bool leftMovement = false; -bool rightMovement = false; - -// Motor control -bool motorEnabled = false; - -// Task handle for motor control -TaskHandle_t motorControlTask; - -// ISR for Timer1 compare match: toggles the step pin if the motor is enabled -ISR(TIMER1_COMPA_vect) { - if (motorEnabled) { - digitalWrite(STEP_PIN, !digitalRead(STEP_PIN)); - } -} - -// Reads and maps joystick input, updates movement flags, and notifies the motor task if needed -void updateInput() { - int joystickValueRaw = analogRead(JOYSTICK_PIN); - int joystickValue = map(joystickValueRaw, 0, 1023, 0, 100); - - leftMovement = (joystickValue > (100 - JOY_CRITICAL_POINT)); - rightMovement = (joystickValue < JOY_CRITICAL_POINT); - - if (leftMovement || rightMovement) { - xTaskNotifyGive(motorControlTask); // Wake up the motor control task - } -} - -// Task to regularly poll the joystick -void JoystickHandler(void *params) { - while (true) { - updateInput(); - vTaskDelay(100 / portTICK_PERIOD_MS); // Poll every 100ms - } -} - -// Task to handle motor control logic -void MotorController(void *params) { - // Configure motor pins - pinMode(STEP_PIN, OUTPUT); - pinMode(DIR_PIN, OUTPUT); - pinMode(EN_PIN, OUTPUT); - - motorControlTask = xTaskGetCurrentTaskHandle(); - - while (true) { - updateInput(); - bool joystickActive = leftMovement || rightMovement; - - digitalWrite(EN_PIN, !joystickActive); - digitalWrite(DIR_PIN, rightMovement); - - if (!joystickActive) { - ulTaskNotifyTake(pdTRUE, portMAX_DELAY); - continue; - } - - motorEnabled = joystickActive; - - if (!leftMovement && !rightMovement) { - motorEnabled = false; - } - - vTaskDelay(20 / portTICK_PERIOD_MS); - } -} - -// Configures Timer1 for compare match interrupts -void setupMotor() { - // Set Timer1 to CTC mode with no prescaler - TCCR1A = 0; - TCCR1B = (1 << WGM12) | (1 << CS10); - - // Compare match value (adjust for desired step rate) - OCR1A = 499; - - // Enable Timer1 compare match interrupt - TIMSK1 |= (1 << OCIE1A); -} +#include "MotorControlPart.h" void setup() { Serial.begin(115200); @@ -100,8 +8,7 @@ void setup() { setupMotor(); // Create tasks - xTaskCreate(JoystickHandler, "Joystick", 200, NULL, 10, NULL); - xTaskCreate(MotorController, "Motor", 200, NULL, 100, NULL); + xTaskCreate(MotorControlTask, "Motor", 200, NULL, 10, NULL); } void loop() { diff --git a/src/MotorControlPart.h b/src/MotorControlPart.h new file mode 100644 index 0000000..2ab5a20 --- /dev/null +++ b/src/MotorControlPart.h @@ -0,0 +1,109 @@ +#include +#include +// Pin used for the joystick +#define JOYSTICK_PIN 63 + +// [X, Y, Head] +#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}; +uint8_t selectedIndex = 1; + +// Critical range for detecting joystick movement +static const int JOY_CRITICAL_POINT = 25; + +bool leftMovement = false; +bool rightMovement = false; + +// Timers for stepping the motors +ISR(TIMER1_COMPA_vect) { + if (motorsEnabled[0]) { + digitalWrite(STEP_PINS[0], !digitalRead(STEP_PINS[0])); + } +} +ISR(TIMER3_COMPA_vect) { + if (motorsEnabled[1]) { + digitalWrite(STEP_PINS[1], !digitalRead(STEP_PINS[1])); + } +} +ISR(TIMER4_COMPA_vect) { + if (motorsEnabled[2]) { + digitalWrite(STEP_PINS[2], !digitalRead(STEP_PINS[2])); + } +} + +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 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++) + { + pinMode(STEP_PINS[i], OUTPUT); + pinMode(DIR_PINS[i], OUTPUT); + pinMode(EN_PINS[i], OUTPUT); + } + + cli(); + // Timer 1 (16-bit) + TCCR1A = 0; + TCCR1B = (1 << WGM12) | (1 << CS10); // CTC mode, no prescaler + OCR1A = 499; // Compare match value + TIMSK1 |= (1 << OCIE1A); // Enable Timer1 Compare A Match Interrupt + + // Timer 3 (16-bit) + TCCR3A = 0; + TCCR3B = (1 << WGM32) | (1 << CS30); // CTC mode, no prescaler + OCR3A = 499; // Compare match value + TIMSK3 |= (1 << OCIE3A); // Enable Timer3 Compare A Match Interrupt + sei(); + + // Timer 4 (16-bit) + TCCR4A = 0; + TCCR4B = (1 << WGM42) | (1 << CS40); // CTC mode, no prescaler + OCR4A = 499; // Compare match value + TIMSK4 |= (1 << OCIE4A); // Enable Timer1 Compare A Match Interrupt +} \ No newline at end of file