Now enters a button protocol when clicked

.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,100 @@
+#include <Arduino.h>
+#ifndef ESP32
+  #include <Arduino_FreeRTOS.h>
+#endif
 #include "StepperController.h"
-#include "StepperXController.h"
+#include "ZCodeParser.h"
-#include "Time.h"
+#include "Shared/MotorMoveParams.h"
-#include <Arduino_FreeRTOS.h>
+#include "Shared/Steppers.h"
+#include "InputController.h"
 
-#define JoystickPin 63
+// [X, Y, Head]
+#define NUM_STEPPERS 3
+Stepper steppers[NUM_STEPPERS] = {
+  {38, 55, 54, 0, 'x', false, false, true},
+  {56, 61, 60, 0, 'y', false, false, true},
+  {30, 34, 36, 0, 'e', false, false, true},
+};
 
-#define grabPin 19
+#pragma region Not gonna mess with this magic
 
-Time time;
+#ifdef ESP32
+// Timer Handles
+hw_timer_t *timer1 = NULL;
+hw_timer_t *timer2 = NULL;
+hw_timer_t *timer3 = NULL;
 
-void setup(){
+// Interrupt Service Routines for each timer
-    // Start serial communication at 115200 baud
+void IRAM_ATTR timer1ISR()
-    Serial.begin(115200);
+{
-    
+  if (motorsEnabled[0])
-    // Set the pin mode for the defined pins
+  {
-    pinMode(grabPin, INPUT_PULLUP);
+    digitalWrite(STEP_PINS[0], !digitalRead(STEP_PINS[0]));
-
-    initSteppers();
-    initXStepper();
-}
-
-bool isGrabbing = false;
-float startGrabbingTime = 0;
-const float moveToDropZoneTime = 1750; // How long to move right (relative to me)
-
-const int joyCriticalPoint = 10;
-
-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);
+void IRAM_ATTR timer2ISR()
-
+{
-  if (digitalRead(grabPin) == LOW){
+  if (motorsEnabled[1])
-      beginGrabSequence();
+  {
-      //initGrabber();
+    digitalWrite(STEP_PINS[1], !digitalRead(STEP_PINS[1]));
   }
-
-  // //Serial.print(millis());
-
-  // Serial.print("Left state: ");
-  // Serial.print(left);
-  // Serial.print("  |  ");
-  // Serial.print("Right state: ");
-  // Serial.println(right);
-
-
-  delay(10);  // delay in milliseconds
 }
 
-void initGrabber(){
+void IRAM_ATTR timer3ISR()
-  moveStepper(StepperAxis::Head, 100000, 1000);
+{
+  if (motorsEnabled[2])
+  {
+    digitalWrite(STEP_PINS[2], !digitalRead(STEP_PINS[2]));
+  }
+}
+#else
+
+// Timers for stepping the motors
+ISR(TIMER3_COMPA_vect) {
+  if (steppers[0].steppingEnabled && steppers[0].moveQueue != 0) {
+    digitalWrite(steppers[0].stepPin, !digitalRead(steppers[0].stepPin));
+  }
+}
+ISR(TIMER4_COMPA_vect) {
+  if (steppers[1].steppingEnabled && steppers[1].moveQueue != 0) {
+    digitalWrite(steppers[1].stepPin, !digitalRead(steppers[1].stepPin));
+  }
+}
+ISR(TIMER5_COMPA_vect) {
+  if (steppers[2].steppingEnabled && steppers[2].moveQueue != 0) {
+    digitalWrite(steppers[2].stepPin, !digitalRead(steppers[2].stepPin));
+  }
+}
+#endif
+
+#pragma endregion
+
+void anotherFunc(void * params){
+
+  while (true)
+  {
+    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 beginGrabSequence(){
+void setup() {
-    moveStepper(StepperAxis::Head, 10000, 100); // open hand
+  Serial.begin(115200);
-    moveStepper(StepperAxis::Y, 10000, -1100);
-    moveStepper(StepperAxis::Head, 1000, -100, 100); // close hand
-    moveStepper(StepperAxis::Y, 10000, 1100);
 
-    isGrabbing = true;
+  StepperController::Init(steppers, NUM_STEPPERS);
-    startGrabbingTime = millis();
+  
+  xTaskCreate(StepperController::MotorTask, "Motors", 1024, NULL, 5,  NULL);
+  // xTaskCreate(anotherFunc, "Other loop", 1024, NULL, 5,  NULL);
+  xTaskCreate(InputControlTask, "Input", 1024, NULL, 5,  NULL);
+}
+
+void loop() {
+  // The RTOS scheduler manages the tasks.
 }

src/InputController.h

@@ -0,0 +1,56 @@
+#include <Arduino.h>
+#ifndef ESP32
+  #include <Arduino_FreeRTOS.h>
+#endif
+#include "StepperController.h"
+
+#define JOYSTICK_PIN 63
+#define JOY_CRITICAL_POINT 25
+
+#define BTN_PIN 19
+
+TaskHandle_t inputTask;
+
+void ClawProtocol(void *params) {
+    Serial.println("Started button protocol");
+
+    StepperController::SetMotorEnabled('y', true);
+    StepperController::SetMovementEnabled('y', true);
+    StepperController::SetMoveQueue('y', 3000);
+
+    vTaskResume(inputTask);
+    Serial.println("Button protocol done");
+    vTaskDelete(NULL);
+}
+
+void InputControlTask(void *params) {
+    inputTask = xTaskGetCurrentTaskHandle();
+    pinMode(BTN_PIN, INPUT_PULLUP); // Setup button pin
+
+  while (true) {
+    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);
+
+    bool isButtonPress = digitalRead(BTN_PIN) == LOW;
+    if (isButtonPress){
+        xTaskCreate(ClawProtocol, "Button protocol", 500, (void*)&inputTask, 100, NULL);
+        vTaskSuspend(inputTask);
+    }
+    
+    if (leftMovement) {
+        StepperController::SetMotorEnabled('x', true);
+        StepperController::SetMovementEnabled('x', true);
+        StepperController::SetMoveQueue('x', 150);
+    }
+
+    if (rightMovement) {
+        StepperController::SetMotorEnabled('x', true);
+        StepperController::SetMovementEnabled('x', true);
+        StepperController::SetMoveQueue('x', -150);
+    }
+
+    vTaskDelay(pdMS_TO_TICKS(100)); // Poll every x
+  }
+}

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/Steppers.h

@@ -0,0 +1,16 @@
+#ifndef STEPPER_H
+#define STEPPER_H
+
+#include <Arduino.h>
+
+struct Stepper {
+    uint8_t enPin;
+    uint8_t dirPin;
+    uint8_t stepPin;
+    int moveQueue;
+    char axis;
+    bool steppingEnabled;
+    bool motorEnabled;
+    bool disableOnIdle;
+};
+#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.cpp

@@ -0,0 +1,141 @@
+#include "StepperController.h"
+
+// Static variable definitions
+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)
+    {
+      // Serial.print("Found motor: ");
+      // Serial.println(i);
+      return i;
+    }
+  }
+  return -1;
+}
+
+void StepperController::Init(Stepper *steppers, int numOfSteppers)
+{
+  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);
+  }
+
+#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 << 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::MotorTask(void *params)
+{
+  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;
+        }
+      }
+
+      // Update motor enable states
+      digitalWrite(stepper->enPin, !stepper->motorEnabled);
+    }
+
+    StepperController::lastUpdateTime = millis();
+    vTaskDelay(pdMS_TO_TICKS(50));
+  }
+  
+}
+
+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

@@ -1,76 +1,37 @@
-#include <SpeedyStepper.h>
+#ifndef STEPPER_CONTROLLER_H
+#define STEPPER_CONTROLLER_H
 
-#define NUM_STEPPERS 3
+#include <Arduino.h>
-// [X, Y, Head]
+#ifndef ESP32
-const int stepPins[NUM_STEPPERS] = {60, 36};
+  #include <Arduino_FreeRTOS.h>
-const int dirPins[NUM_STEPPERS] = {61, 34};
+#endif
-const int enablePins[NUM_STEPPERS] = {56, 30};
+#include <Shared/ZCommand.h>
+#include <Shared/MotorMoveParams.h>
+#include <Shared/Steppers.h>
+#include <algorithm>
 
+class StepperController
+{
+private:
+    static int NumOfSteppers;
+    static Stepper *Steppers;
+    static long lastUpdateTime;
 
-enum StepperAxis{
+    static int getStepperIndex(const char axis);
-    Y,
+
-    Head
+public:
+    static void Init(Stepper *steppers, int numOfSteppers = 3);
+
+    static void MotorTask(void * params);
+
+    static void SetMovementEnabled(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);
+
+    static void SetMotorEnabled(const char axis, bool enabled);
 };
 
-SpeedyStepper steppers[NUM_STEPPERS];
-bool isMoving[NUM_STEPPERS];
 
-// void stepperLoop(){
+#endif
-//     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();
-};