still errors but im falling asleep :(

.vscode/settings.json

@@ -2,6 +2,56 @@
     "files.associations": {
         "*.js": "javascript",
         "*.h": "cpp",
-        "string": "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

@@ -10,9 +10,10 @@
 
 
 [env:mega2560]
-platform = atmelave
+platform = atmelavr
 board = megaatmega2560
 framework = arduino
 lib_extra_dirs = ~/Documents/Arduino/libraries
 lib_deps = 
     FreeRTOS
+upload_speed = 115200

src/ClawMachineOverhaul.ino

@@ -1,16 +1,18 @@
 #include <Arduino.h>
-#include <Arduino_FreeRTOS.h>
+// #include <Arduino_FreeRTOS.h>
 #include "MotorControlPart.h"
 #include "ZCodeParser.h"
 
 void setup() {
   Serial.begin(115200);
 
+  delayMicroseconds(1000);
+
   // setupMotor();
 
   // Create tasks
   // xTaskCreate(MotorControlTask, "Motor", 200, NULL, 10,  NULL);
-  ZCodeParser::ParseString("G0 Z2 ", void, void);
+  ZCodeParser::ParseString("G0 E20 X40; Comment test\nG1 Y50 X69\nG0 Y235 E5532");
 }
 
 void loop() {

src/ZCodeParser.h

@@ -2,44 +2,73 @@
 #include <vector>
 
 typedef struct {
-    char Axis;
-    int time;
-} MoveCommand;
+    String command;
+    std::vector<String> params;
+} GCommand;
 
-class ZCodeParser
+class ZCodeParser // Variant of GCode, but made by Zhen. Lol 
 {
 private:
+    static std::vector<String> splitString(const String &input, char delimiter);
 public:
-    static void ParseString(String inString, MoveCommand *outMovecommands, int *numOfCommands);
+    static std::vector<GCommand> ParseString(const String inString);
 };
 
-void ZCodeParser::ParseString(String inString, MoveCommand *outMovecommands, int *numOfCommands){
-    // G0 = move, no block
-    // G1 = move, block
-    // ;  = comments
-    // \n = new command
-
-    std::vector<String> stringCommands;
+std::vector<String> ZCodeParser::splitString(const String &input, char delimiter) {
+    std::vector<String> result;
     int startIndex = 0;
     int endIndex;
-    while (true)
-    {
-        endIndex = inString.indexOf('\n', startIndex);
 
-        if (endIndex == -1){
+    while (true) {
+        endIndex = input.indexOf(delimiter, startIndex);
+        if (endIndex == -1) {
+            result.push_back(input.substring(startIndex)); // Get the rest of the string
             break;
         }
+        result.push_back(input.substring(startIndex, endIndex));
+        startIndex = endIndex + 1;
+    }
 
-        stringCommands.push_back(inString.substring(startIndex, endIndex));
-        startIndex = endIndex;
+    return result;
+}
+
+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<String> stringCommands = ZCodeParser::splitString(inString, '\n');
+    
+    // Remove comments
+    for (size_t i = 0; i < stringCommands.size(); i++)
+    {
+        int commentIndex = stringCommands[i].indexOf(';');
+        if (commentIndex == -1) continue; // Continue if no comments
+
+        stringCommands[i] = stringCommands[i].substring(0, commentIndex);
+    }
+
+    // Extract function name and params
+    std::vector<GCommand> commnads;
+    for (size_t i = 0; i < stringCommands.size(); i++)
+    {
+        std::vector<String> seperatedStrings = ZCodeParser::splitString(stringCommands[i], ' ');
+        GCommand commnad;
+        commnad.command = seperatedStrings[0];
+        stringCommands.erase(stringCommands.begin());
+        commnad.params = stringCommands;
     }
 
     Serial.println("Parsed: ");
-    for (size_t i = 0; i < stringCommands.size(); i++)
+    for (size_t i = 0; i < commnads.size(); i++)
     {
-        Serial.print(stringCommands[i]);
+        Serial.print(commnads[i].command);
         Serial.print(", ");
     }
     Serial.println("\n");
-    
+
+    // return ;   
 }