Recovered lost changes. ESP32 supported

platformio.ini

@@ -16,4 +16,25 @@ framework = arduino
 lib_extra_dirs = ~/Documents/Arduino/libraries
 lib_deps = 
     FreeRTOS
-upload_speed = 115200
+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,20 +1,24 @@
 #include <Arduino.h>
-// #include <Arduino_FreeRTOS.h>
-#include "MotorControlPart.h"
+#ifndef ESP32
+  #include <Arduino_FreeRTOS.h>
+#endif
+// #include "MotorControlPart.h"
 #include "ZCodeParser.h"
 
 void setup() {
   Serial.begin(115200);
 
-  delayMicroseconds(1000);
+  delay(1000);
 
   // setupMotor();
 
   // Create tasks
   // xTaskCreate(MotorControlTask, "Motor", 200, NULL, 10,  NULL);
-  ZCodeParser::ParseString("G0 E20 X40; Comment test\nG1 Y50 X69\nG0 Y235 E5532");
 }
 
 void loop() {
   // The RTOS scheduler manages the tasks. No code is needed here.
+
+  ZCodeParser::ParseString("G0 E20 X40; Comment test\nG1 Y50 X69\nG0 Y235 E5532");
+  delay(3000);
 }

src/MotorControlPart.h

@@ -1,5 +1,9 @@
 #include <Arduino.h>
+#ifndef ESP32
 #include <Arduino_FreeRTOS.h>
+#endif
+#include <Shared/ZCommand.h>
+
 // Pin used for the joystick
 #define JOYSTICK_PIN 63
 
@@ -17,6 +21,38 @@ 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(TIMER1_COMPA_vect) {
   if (motorsEnabled[0]) {
@@ -33,6 +69,7 @@ ISR(TIMER4_COMPA_vect) {
     digitalWrite(STEP_PINS[2], !digitalRead(STEP_PINS[2]));
   }
 }
+#endif
 
 void updateInput() {
   int joystickValueRaw = analogRead(JOYSTICK_PIN);
@@ -52,6 +89,15 @@ void updateInput() {
   // Serial.print(" | Motor: "); Serial.println(motorsEnabled[selectedIndex] ? "ON" : "OFF");
 }
 
+void ExecuteCommand(void *params){
+  ZCommand receivedValue = *((ZCommand *)params);
+}
+
+void MoveMotor(void *params)
+{
+  int receivedValue = *((ZCommand *)params);
+}
+
 void MotorControlTask(void *params) {
   while (true) {
     updateInput();
@@ -87,6 +133,25 @@ void setupMotor() {
         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 1 (16-bit)
     TCCR1A = 0;
@@ -99,11 +164,12 @@ void setupMotor() {
     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
+    sei();
+    #endif
 }

src/Shared/ZCommand.h

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

src/ZCodeParser.h

@@ -1,74 +1,105 @@
 #include <Arduino.h>
 #include <vector>
+#include <Shared/ZCommand.h>
 
-typedef struct {
-    String command;
-    std::vector<String> params;
-} GCommand;
-
-class ZCodeParser // Variant of GCode, but made by Zhen. Lol 
+class ZCodeParser
 {
 private:
-    static std::vector<String> splitString(const String &input, char delimiter);
+    static void splitString(const char *input, char delimiter, std::vector<String> &result);
+
 public:
-    static std::vector<GCommand> ParseString(const String inString);
+    static std::vector<ZCommand> ParseString(const String &inString);
 };
 
-std::vector<String> ZCodeParser::splitString(const String &input, char delimiter) {
-    std::vector<String> result;
-    int startIndex = 0;
-    int endIndex;
+void ZCodeParser::splitString(const char *input, char delimiter, std::vector<String> &result)
+{
+    // Serial.println("Splitting String: "); 
 
-    while (true) {
-        endIndex = input.indexOf(delimiter, startIndex);
-        if (endIndex == -1) {
-            result.push_back(input.substring(startIndex)); // Get the rest of the string
-            break;
+    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.emplace_back(String(input + start, i - start)); // Efficient substring creation
+            }
+            start = i + 1;
         }
-        result.push_back(input.substring(startIndex, endIndex));
-        startIndex = endIndex + 1;
     }
 
-    return result;
+    // Serial.println("Split Result:");
+    // for (const auto &s : result)
+    // {
+    //     Serial.print("[" + s + "] ");
+    // }
+    // Serial.println();
 }
 
-std::vector<GCommand> ZCodeParser::ParseString(const String inString){
-    // G0  = move, no block
-    // G1  = move, block
-    // M17 = Enable Stepper
-    // M18 = Disable Stepper
-    // ;   = comments
-    // \n  = new command
+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 = ZCodeParser::splitString(inString, '\n');
+    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 (size_t i = 0; i < stringCommands.size(); i++)
+    for (String &command : stringCommands)
     {
-        int commentIndex = stringCommands[i].indexOf(';');
-        if (commentIndex == -1) continue; // Continue if no comments
-
-        stringCommands[i] = stringCommands[i].substring(0, commentIndex);
+        int commentIndex = command.indexOf(';');
+        if (commentIndex != -1)
+        {
+            command = command.substring(0, commentIndex);
+            // Serial.println("Comment Removed: " + command);
+        }
     }
 
-    // Extract function name and params
-    std::vector<GCommand> commnads;
-    for (size_t i = 0; i < stringCommands.size(); i++)
+    std::vector<ZCommand> commands;
+    for (const String &commandLine : stringCommands)
     {
-        std::vector<String> seperatedStrings = ZCodeParser::splitString(stringCommands[i], ' ');
-        GCommand commnad;
-        commnad.command = seperatedStrings[0];
-        stringCommands.erase(stringCommands.begin());
-        commnad.params = 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.emplace_back(std::move(separatedStrings[j]));
+            // Serial.println("Added Param: " + command.params.back());
+        }
+
+        commands.emplace_back(std::move(command));
+        // Serial.println("Stored Command: " + commands.back().command);
     }
 
-    Serial.println("Parsed: ");
-    for (size_t i = 0; i < commnads.size(); i++)
+    Serial.println("Parsed Commands:");
+    for (const auto &cmd : commands)
     {
-        Serial.print(commnads[i].command);
-        Serial.print(", ");
+        Serial.print(cmd.command + ": ");
+        for (const auto &param : cmd.params)
+        {
+            Serial.print(param + " ");
+    }
+        Serial.println();
     }
-    Serial.println("\n");
 
-    // return ;   
-}
+    // Serial.println("Heap After Parsing: " + String(ESP.getFreeHeap()));
+    // Serial.println("--- Parsing Finished ---\n");
+
+    return commands;
+}