Realised that i made this too broad

.vscode/settings.json

@@ -0,0 +1,57 @@
+{
+    "files.associations": {
+        "*.js": "javascript",
+        "*.h": "cpp",
+        "string": "cpp",
+        "vector": "cpp",
+        "new": "cpp",
+        "algorithm": "cpp",
+        "associative_base": "cpp",
+        "basic_definitions": "cpp",
+        "bitset": "cpp",
+        "cctype": "cpp",
+        "char_traits": "cpp",
+        "cmath": "cpp",
+        "complex": "cpp",
+        "cstddef": "cpp",
+        "cstdint": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "deque": "cpp",
+        "exception": "cpp",
+        "func_exception": "cpp",
+        "functional": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "ios": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "istream_helpers": "cpp",
+        "iterator": "cpp",
+        "iterator_base": "cpp",
+        "limits": "cpp",
+        "list": "cpp",
+        "locale": "cpp",
+        "map": "cpp",
+        "memory": "cpp",
+        "numeric": "cpp",
+        "ostream": "cpp",
+        "ostream_helpers": "cpp",
+        "queue": "cpp",
+        "serstream": "cpp",
+        "set": "cpp",
+        "sstream": "cpp",
+        "stack": "cpp",
+        "stdexcept": "cpp",
+        "streambuf": "cpp",
+        "string_iostream": "cpp",
+        "type_traits": "cpp",
+        "typeinfo": "cpp",
+        "utility": "cpp",
+        "valarray": "cpp"
+    }
+}

platformio.ini

@@ -11,8 +11,30 @@
 
 [env:mega2560]
 platform = atmelavr
-board = megaatmega2560
+board = megaatmega2560 
 framework = arduino
 lib_extra_dirs = ~/Documents/Arduino/libraries
 lib_deps = 
     FreeRTOS
+upload_speed = 115200
+
+[env:esp32-pico]
+platform = espressif32
+board = esp32dev
+framework = arduino
+; lib_extra_dirs = ~/Documents/Arduino/libraries
+lib_deps = 
+    Arduino
+    vector
+upload_speed = 921600
+monitor_speed = 115200
+build_type = debug
+build_flags = 
+    -Og 
+    -g3 
+    -ggdb
+    -DCORE_DEBUG_LEVEL=5
+    -std=gnu++17
+debug_tool = esp-prog
+debug_init_break = tbreak setup
+upload_protocol = esptool

src/ClawMachineOverhaul.ino

@@ -1,84 +1,33 @@
-#include "StepperController.h"
-#include "StepperXController.h"
-#include "Time.h"
-#include <Arduino_FreeRTOS.h>
+#include <Arduino.h>
+#ifndef ESP32
+  #include <Arduino_FreeRTOS.h>
+#endif
+#include "MotorControlPart.h"
+#include "ZCodeParser.h"
+#include "Shared/MotorMoveParams.h"
 
-#define JoystickPin 63
+void anotherFunc(void * params){
+  MotorMoveParams *pam = (MotorMoveParams *)malloc(sizeof(MotorMoveParams));
+  pam->motorChar = 'x';
+  pam->moveTimeMs = 3000;
+  xTaskCreate(MoveMotor, "Motor", 1024, (void *)pam, 10,  NULL);
 
-#define grabPin 19
-
-Time time;
-
-void setup(){
-    // Start serial communication at 115200 baud
-    Serial.begin(115200);
-    
-    // Set the pin mode for the defined pins
-    pinMode(grabPin, INPUT_PULLUP);
-
-    initSteppers();
-    initXStepper();
+  vTaskDelay(pdMS_TO_TICKS(50)); // Allow time for other task to accept params
+  free(pam);
+  vTaskDelete(NULL);
 }
 
-bool isGrabbing = false;
-float startGrabbingTime = 0;
-const float moveToDropZoneTime = 1750; // How long to move right (relative to me)
+void setup() {
+  Serial.begin(115200);
 
-const int joyCriticalPoint = 10;
+  setupMotor();
 
-void loop(){
-  // Updates deltatime
-  time.update();
-
-  stepperXLoop();
-
-  // Joystick. TODO: Make this only run when needed. Too lazy now.
-  int joystickValueRaw = analogRead(JoystickPin);
-  int joystickValue = map(joystickValueRaw, 0, 1023, 0, 100);
-  bool left = joystickValue > 100-joyCriticalPoint;
-  bool right = joystickValue < joyCriticalPoint;
-
-  // Serial.print("Left: ");
-  // Serial.print(left);
-  // Serial.print("    Right: ");
-  // Serial.println(right);
-
-  if (isGrabbing && millis() > startGrabbingTime + moveToDropZoneTime) {
-    isGrabbing = false;
-    processJoystickInput(false, false); // Quickly stop motor
-    delay(1000);
-    moveStepper(StepperAxis::Head, 10000, 100); // open hand
-  }
-
-  processJoystickInput(left, right);
-
-  if (digitalRead(grabPin) == LOW){
-      beginGrabSequence();
-      //initGrabber();
-  }
-
-  // //Serial.print(millis());
-
-  // Serial.print("Left state: ");
-  // Serial.print(left);
-  // Serial.print("  |  ");
-  // Serial.print("Right state: ");
-  // Serial.println(right);
-
-
-  delay(10);  // delay in milliseconds
+  // Create tasks
+  xTaskCreate(anotherFunc, "Other loop", 1024, NULL, 5,  NULL);
 }
 
-void initGrabber(){
-  moveStepper(StepperAxis::Head, 100000, 1000);
-}
+void loop() {
+  // The RTOS scheduler manages the tasks. No code is needed here.
 
-void beginGrabSequence(){
-    moveStepper(StepperAxis::Head, 10000, 100); // open hand
-    moveStepper(StepperAxis::Y, 10000, -1100);
-    moveStepper(StepperAxis::Head, 1000, -100, 100); // close hand
-    moveStepper(StepperAxis::Y, 10000, 1100);
-
-    isGrabbing = true;
-    startGrabbingTime = millis();
+  // ZCodeParser::ParseString("G0 E20 X40; Comment test\nG1 Y50 X69\nG0 Y235 E5532");
 }

src/MotorControlPart.h

@@ -0,0 +1,209 @@
+#include <Arduino.h>
+#ifndef ESP32
+#include <Arduino_FreeRTOS.h>
+#endif
+#include <Shared/ZCommand.h>
+#include <Shared/MotorMoveParams.h>
+#include <math.h>
+
+// Pin used for the joystick
+#define JOYSTICK_PIN 63
+
+struct CharMapping {
+    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;
+
+// Critical range for detecting joystick movement
+static const int JOY_CRITICAL_POINT = 25;
+
+bool leftMovement  = false;
+bool rightMovement = false;
+
+#ifdef ESP32
+// Timer Handles
+hw_timer_t *timer1 = NULL;
+hw_timer_t *timer2 = NULL;
+hw_timer_t *timer3 = NULL;
+
+// Interrupt Service Routines for each timer
+void IRAM_ATTR timer1ISR()
+{
+  if (motorsEnabled[0])
+  {
+    digitalWrite(STEP_PINS[0], !digitalRead(STEP_PINS[0]));
+  }
+}
+
+void IRAM_ATTR timer2ISR()
+{
+  if (motorsEnabled[1])
+  {
+    digitalWrite(STEP_PINS[1], !digitalRead(STEP_PINS[1]));
+  }
+}
+
+void IRAM_ATTR timer3ISR()
+{
+  if (motorsEnabled[2])
+  {
+    digitalWrite(STEP_PINS[2], !digitalRead(STEP_PINS[2]));
+  }
+}
+#else
+
+// Timers for stepping the motors
+ISR(TIMER3_COMPA_vect) {
+  if (motorsEnabled[0]) {
+    digitalWrite(STEP_PINS[0], !digitalRead(STEP_PINS[0]));
+  }
+}
+ISR(TIMER4_COMPA_vect) {
+  if (motorsEnabled[1]) {
+    digitalWrite(STEP_PINS[1], !digitalRead(STEP_PINS[1]));
+  }
+}
+ISR(TIMER5_COMPA_vect) {
+  if (motorsEnabled[2]) {
+    digitalWrite(STEP_PINS[2], !digitalRead(STEP_PINS[2]));
+  }
+}
+#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);
+}
+
+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.
+
+  // Enable motor and set direction
+  motorsEnabled[index] = true;
+  digitalWrite(EN_PINS[index], !motorsEnabled[index]); // Inverted for some reason
+  digitalWrite(DIR_PINS[index], isBackwards);
+  
+  vTaskDelay(pdMS_TO_TICKS(moveTimeMs));
+
+  // Stop motor
+  motorsEnabled[index] = false;
+  digitalWrite(EN_PINS[index], !motorsEnabled[index]); // 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++)
+    {
+        pinMode(STEP_PINS[i], OUTPUT);
+        pinMode(DIR_PINS[i], OUTPUT);
+        pinMode(EN_PINS[i], 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
+}

src/Shared/MotorMoveParams.h

@@ -0,0 +1,9 @@
+#ifndef MOTOR_MOVE_PARAMS_H
+#define MOTOR_MOVE_PARAMS_H
+
+typedef struct {
+    char motorChar;
+    float moveTimeMs;
+} MotorMoveParams;
+
+#endif

src/Shared/ZCommand.h

@@ -0,0 +1,12 @@
+#ifndef ZCOMMAND_H
+#define ZCOMMAND_H
+#include <Arduino.h>
+#include <vector>
+
+typedef struct
+{
+    String command;
+    std::vector<String> params;
+} ZCommand;
+
+#endif

src/StepperController.h

@@ -1,76 +0,0 @@
-#include <SpeedyStepper.h>
-
-#define NUM_STEPPERS 3
-// [X, Y, Head]
-const int stepPins[NUM_STEPPERS] = {60, 36};
-const int dirPins[NUM_STEPPERS] = {61, 34};
-const int enablePins[NUM_STEPPERS] = {56, 30};
-
-
-enum StepperAxis{
-    Y,
-    Head
-};
-
-SpeedyStepper steppers[NUM_STEPPERS];
-bool isMoving[NUM_STEPPERS];
-
-// void stepperLoop(){
-//     for (int i = 0; i < NUM_STEPPERS; i++) {
-//         SpeedyStepper* selectedStepper = &(steppers[i]);
-//         if (!selectedStepper->motionComplete())
-//         {
-//             selectedStepper->processMovement();
-//         }
-//     }
-// }
-
-void moveStepperByIndex(int stepperIndex, int speed, int dist, int accel = 2000){
-    isMoving[stepperIndex] = true;
-
-    SpeedyStepper* selectedStepper = &(steppers[stepperIndex]);
-    selectedStepper->setAccelerationInMillimetersPerSecondPerSecond(accel);
-    selectedStepper->setSpeedInStepsPerSecond(speed);
-
-    selectedStepper->setupRelativeMoveInMillimeters(dist);
-
-    while (!selectedStepper->motionComplete())
-    {
-        selectedStepper->processMovement();
-    }
-}
-
-void moveStepper(StepperAxis axis, int speed, int dist, int accel = 2000) {moveStepperByIndex(axis, speed, dist, accel);}
-
-void stopStepperByIndex(int stepperIndex){
-    if (!isMoving[stepperIndex]) return;
-
-    if (!steppers[stepperIndex].motionComplete()) return;
-    
-    steppers[stepperIndex].setupStop();
-    isMoving[stepperIndex] = false;
-}
-void stopStepper(StepperAxis axis){stopStepperByIndex(axis);}
-
-void initSteppers(){
-    for (int i = 0; i < NUM_STEPPERS; i++) {
-        pinMode(stepPins[i], OUTPUT);
-        pinMode(dirPins[i], OUTPUT);
-        pinMode(enablePins[i], OUTPUT);
- 
-        // Enable the stepper motor
-        digitalWrite(enablePins[i], HIGH); // Assuming HIGH disables the motor
-    }
-
-    for (int i = 0; i < NUM_STEPPERS; i++) {
-        digitalWrite(enablePins[i], LOW); // Assuming LOW enables the motor
-
-        // Attach the stepper motor to the pins
-        steppers[i].connectToPins(stepPins[i], dirPins[i]);
-        steppers[i].setStepsPerRevolution(200);
-    }
-
-    moveStepper(StepperAxis::Y, 10000, 1000);
-    moveStepper(StepperAxis::Head, 10000, 100);
-
-}

src/StepperXController.h

@@ -1,34 +0,0 @@
-#include <ContinuousStepper.h>
-#include <ContinuousStepper/Tickers/Tone.hpp>
-
-ContinuousStepper<StepperDriver, ToneTicker> stepper;
-
-void initXStepper(){
-    stepper.begin(/*step=*/54, /*dir=*/55);
-    stepper.setAcceleration(10000000);
-    pinMode(38, OUTPUT);
-}
-
-void stepperXLoop(){
-    stepper.loop();
-}
-
-bool isMovingLeft = false;
-void processJoystickInput(bool left, bool right){
-    if (left){
-        // if (!isMovingLeft){
-        //     isMovingLeft = true;
-        //     stepper.stop();
-        // }
-        stepper.spin(14000);
-    }
-    else if (right){
-        // if (isMovingLeft){
-        //     isMovingLeft = false;
-        //     stepper.stop();
-        // }
-        stepper.spin(-14000);
-    }
-    else
-        stepper.stop();
-}

src/Time.h

@@ -1,21 +0,0 @@
-class Time {
-private:
-    unsigned long lastTime;
-    float m_deltaTime;
-
-public:
-    Time() {
-        lastTime = millis();
-        m_deltaTime = 0.0f;
-    }
-
-    void update() {
-        unsigned long currentTime = millis();
-        m_deltaTime = (currentTime - lastTime) / 1000.0f; // Convert to seconds
-        lastTime = currentTime;
-    }
-
-    float deltaTime() {
-        return m_deltaTime;
-    }
-};

src/ZCodeParser.h

@@ -0,0 +1,94 @@
+#include <Arduino.h>
+#include <vector>
+#include <Shared/ZCommand.h>
+
+class ZCodeParser
+{
+private:
+    static void splitString(const char *input, char delimiter, std::vector<String> &result);
+
+public:
+    static std::vector<ZCommand> ParseString(const String &inString);
+};
+
+void ZCodeParser::splitString(const char *input, char delimiter, std::vector<String> &result)
+{
+    // Serial.println("Splitting String: "); 
+
+    result.clear();
+    size_t start = 0;
+    size_t len = strlen(input);
+
+    for (size_t i = 0; i <= len; i++)
+    {
+        if (input[i] == delimiter || input[i] == '\0')
+        {
+            if (i > start)
+            {
+                result.push_back(String(input + start).substring(0, i - start)); // Fix applied here
+            }
+            start = i + 1;
+        }
+    }
+
+    // Serial.println("Split Result:");
+    // for (const auto &s : result)
+    // {
+    //     Serial.print("[" + s + "] ");
+    // }
+    // Serial.println();
+}
+
+std::vector<ZCommand> ZCodeParser::ParseString(const String &inString)
+{
+    // Serial.println("\n--- Parsing Started ---");
+    // Serial.println("Heap Before Parsing: " + String(ESP.getFreeHeap()));
+
+    std::vector<String> stringCommands;
+    splitString(inString.c_str(), '\n', stringCommands);
+
+    // Serial.println("After Splitting into Commands:");
+    // for (size_t i = 0; i < stringCommands.size(); i++)
+    // {
+    //     Serial.println("Command[" + String(i) + "]: " + stringCommands[i]);
+    // }
+    
+    // Remove comments
+    for (String &command : stringCommands)
+    {
+        int commentIndex = command.indexOf(';');
+        if (commentIndex != -1)
+        {
+            command = command.substring(0, commentIndex);
+            // Serial.println("Comment Removed: " + command);
+        }
+    }
+
+    std::vector<ZCommand> commands;
+    for (const String &commandLine : stringCommands)
+    {
+        std::vector<String> separatedStrings;
+        splitString(commandLine.c_str(), ' ', separatedStrings);
+
+        if (separatedStrings.empty())
+            continue;
+
+        ZCommand command;
+        command.command = separatedStrings[0];
+
+        // Serial.println("Processing Command: " + command.command);
+        for (size_t j = 1; j < separatedStrings.size(); j++)
+        {
+            command.params.push_back(separatedStrings[j]);
+            // Serial.println("Added Param: " + command.params.back());
+        }
+
+        commands.push_back(command);
+        // Serial.println("Stored Command: " + commands.back().command);
+    }
+
+    // Serial.println("Heap After Parsing: " + String(ESP.getFreeHeap()));
+    // Serial.println("--- Parsing Finished ---\n");
+
+    return commands;
+}

src/classes/CommandTypes.h

@@ -1,7 +0,0 @@
-enum CommandType{
-    Move,
-    Chain,
-    Power,
-    Speed,
-    Accel
-};

src/classes/ICommand.h

@@ -1,6 +0,0 @@
-#include "CommandTypes.h"
-
-class ICommand {
-    public:
-        CommandType getCommandType();
-};