From e3a668fe7cb54f4e00fd8e89dad059dbb3dadf21 Mon Sep 17 00:00:00 2001
From: BOTAlex <zhentao2004@gmail.com>
Date: Wed, 22 Jan 2025 04:29:06 +0100
Subject: [PATCH] Progress

---
 platformio.ini               |  2 +-
 src/ClawMachineOverhaul.ino  | 12 +++++++-----
 src/MotorControlPart.h       | 27 ++++++++++++++++++++++++++-
 src/Shared/MotorMoveParams.h |  9 +++++++++
 src/Shared/ZCommand.h        |  4 ++++
 src/ZCodeParser.h            | 17 +++--------------
 6 files changed, 50 insertions(+), 21 deletions(-)
 create mode 100644 src/Shared/MotorMoveParams.h

diff --git a/platformio.ini b/platformio.ini
index 4568a8f..6fc6913 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -11,7 +11,7 @@
 
 [env:mega2560]
 platform = atmelavr
-board = megaatmega2560
+board = megaatmega2560 
 framework = arduino
 lib_extra_dirs = ~/Documents/Arduino/libraries
 lib_deps = 
diff --git a/src/ClawMachineOverhaul.ino b/src/ClawMachineOverhaul.ino
index 22000af..9d008a8 100644
--- a/src/ClawMachineOverhaul.ino
+++ b/src/ClawMachineOverhaul.ino
@@ -2,14 +2,13 @@
 #ifndef ESP32
   #include <Arduino_FreeRTOS.h>
 #endif
-// #include "MotorControlPart.h"
+#include "MotorControlPart.h"
 #include "ZCodeParser.h"
+#include "Shared/MotorMoveParams.h"
 
 void setup() {
   Serial.begin(115200);
 
-  delay(1000);
-
   // setupMotor();
 
   // Create tasks
@@ -19,6 +18,9 @@ void setup() {
 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);
+  // ZCodeParser::ParseString("G0 E20 X40; Comment test\nG1 Y50 X69\nG0 Y235 E5532");
+  MotorMoveParams pam;
+  pam.motorChar = 'x';
+  xTaskCreate(MoveMotor, "Motor", 200, NULL, 10,  NULL);
+  delay(5000);
 }
diff --git a/src/MotorControlPart.h b/src/MotorControlPart.h
index 6ecad2b..3edddd9 100644
--- a/src/MotorControlPart.h
+++ b/src/MotorControlPart.h
@@ -3,16 +3,24 @@
 #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]
 #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[] = {{'a', 0}, {'b', 1}, {'c', 2}};
 uint8_t selectedIndex = 1;
 
 // Critical range for detecting joystick movement
@@ -71,6 +79,15 @@ ISR(TIMER4_COMPA_vect) {
 }
 #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);
@@ -95,7 +112,15 @@ void ExecuteCommand(void *params){
 
 void MoveMotor(void *params)
 {
-  int receivedValue = *((ZCommand *)params);
+  MotorMoveParams *motorParams = ((MotorMoveParams *) params);
+  char motorChar = motorParams->motorChar;
+  float moveTime = motorParams->moveTime;
+
+  int index = charToMotorIndex(motorChar);
+  int isBackwards = signbit(moveTime); // Negative, then move backwards. Positive, then move forward.
+
+  digitalWrite(EN_PINS[index], !motorsEnabled[index]); // Inverted for some reason
+  digitalWrite(DIR_PINS[index], isBackwards);
 }
 
 void MotorControlTask(void *params) {
diff --git a/src/Shared/MotorMoveParams.h b/src/Shared/MotorMoveParams.h
new file mode 100644
index 0000000..737b566
--- /dev/null
+++ b/src/Shared/MotorMoveParams.h
@@ -0,0 +1,9 @@
+#ifndef MOTOR_MOVE_PARAMS_H
+#define MOTOR_MOVE_PARAMS_H
+
+typedef struct {
+    char motorChar;
+    float moveTime;
+} MotorMoveParams;
+
+#endif
\ No newline at end of file
diff --git a/src/Shared/ZCommand.h b/src/Shared/ZCommand.h
index d5babc6..5add8ee 100644
--- a/src/Shared/ZCommand.h
+++ b/src/Shared/ZCommand.h
@@ -1,3 +1,5 @@
+#ifndef ZCOMMAND_H
+#define ZCOMMAND_H
 #include <Arduino.h>
 #include <vector>
 
@@ -6,3 +8,5 @@ typedef struct
     String command;
     std::vector<String> params;
 } ZCommand;
+
+#endif
\ No newline at end of file
diff --git a/src/ZCodeParser.h b/src/ZCodeParser.h
index 43d8f11..9e2c3d0 100644
--- a/src/ZCodeParser.h
+++ b/src/ZCodeParser.h
@@ -25,7 +25,7 @@ void ZCodeParser::splitString(const char *input, char delimiter, std::vector<Str
         {
             if (i > start)
             {
-                result.emplace_back(String(input + start, i - start)); // Efficient substring creation
+                result.push_back(String(input + start).substring(0, i - start)); // Fix applied here
             }
             start = i + 1;
         }
@@ -79,25 +79,14 @@ std::vector<ZCommand> ZCodeParser::ParseString(const String &inString)
         // Serial.println("Processing Command: " + command.command);
         for (size_t j = 1; j < separatedStrings.size(); j++)
         {
-            command.params.emplace_back(std::move(separatedStrings[j]));
+            command.params.push_back(separatedStrings[j]);
             // Serial.println("Added Param: " + command.params.back());
         }
 
-        commands.emplace_back(std::move(command));
+        commands.push_back(command);
         // Serial.println("Stored Command: " + commands.back().command);
     }
 
-    Serial.println("Parsed Commands:");
-    for (const auto &cmd : commands)
-    {
-        Serial.print(cmd.command + ": ");
-        for (const auto &param : cmd.params)
-        {
-            Serial.print(param + " ");
-    }
-        Serial.println();
-    }
-
     // Serial.println("Heap After Parsing: " + String(ESP.getFreeHeap()));
     // Serial.println("--- Parsing Finished ---\n");