move command execution out of main.cpp

Firmware/EggbotWireless/include/Executor.h

@@ -0,0 +1,22 @@
+#ifndef EXECUTOR_H
+#define EXECUTOR_H
+
+#include <Arduino.h>
+#include <Wire.h>
+
+#include "common/Commands.h"
+
+
+class Executor
+{
+private:
+    /* data */
+public:
+    Executor(/* args */);
+    void execCommand(Command command);
+    I2CStatusMsg status();
+};
+
+
+
+#endif 

Firmware/EggbotWireless/include/Globals.h

@@ -3,7 +3,9 @@
 
 #include <Arduino.h>
 #include "Power.h"
+#include "Executor.h"
 
 Power power;
+Executor executor;
 
 #endif // GLOBALS_H

Firmware/EggbotWireless/src/Executor.cpp

@@ -0,0 +1,76 @@
+#include "Executor.h"
+
+Executor::Executor() {}
+
+void Executor::execCommand(Command command) {
+    if (command.type != CommandType::unk) {
+        Wire.beginTransmission(8);
+        byte buffer[7][sizeof(float)];
+        command.toBytes(buffer[0]);
+        for (int i = 0; i < 7; i++) {
+            /*
+            float dbg;
+            bytesToFloat(&dbg, buffer[i]);
+            Serial.println(dbg);
+             */
+            Wire.write(buffer[i], sizeof(float));
+        }
+        Wire.endTransmission();
+    } else {
+        Serial.println("OK");
+        return;
+    }
+
+    if (command.type == CommandType::G01 || command.type == CommandType::G00) {
+        return;
+    }
+
+    if (command.type == CommandType::M99) {
+        Wire.requestFrom(8, 5 * sizeof(float));
+
+        float resp[5];
+        byte buffer[sizeof(float)];
+
+        for (int i = 0; i < 5; i++) {
+            for (unsigned int j = 0; j < sizeof(float); j++) {
+                while (!Wire.available()) {
+                }
+                buffer[j] = Wire.read();
+            }
+            bytesToFloat(&resp[i], buffer);
+        }
+
+        Serial.println("Status:");
+        Serial.print("X: ");
+        Serial.println(resp[servoRot]);
+
+        Serial.print("Y: ");
+        Serial.println(resp[eggRot]);
+
+        Serial.print("Xmm: ");
+        Serial.println(resp[servoPos]);
+
+        Serial.print("Ymm: ");
+        Serial.println(resp[eggPos]);
+
+        Serial.print("PEN: ");
+        Serial.println(resp[penPos]);
+
+        return;
+    }
+}
+
+I2CStatusMsg Executor::status() {
+    unsigned long reqTime = millis();
+    Wire.requestFrom(8, 1);
+
+    while (!Wire.available()) {
+        if (millis() - reqTime > 100) {
+            Wire.requestFrom(8, 1);
+            reqTime = millis();
+        }
+    }
+
+    int resp = Wire.read();
+    return static_cast<I2CStatusMsg>(resp);
+}

Firmware/EggbotWireless/src/main.cpp

@@ -2,73 +2,12 @@
 #include <ESP8266WiFi.h>
 #include <Ticker.h>
 #include <Wire.h>
+#include "GCodeParser.h"
 #include "Globals.h"
 #include "Power.h"
-#include "GCodeParser.h"
 #include "common/Commands.h"
 
 String inString;
-bool waitingForNext = false;
-
-void execCommand(Command command) {
-    if (command.type != CommandType::unk) {
-        Wire.beginTransmission(8);
-        byte buffer[7][sizeof(float)];
-        command.toBytes(buffer[0]);
-        for (int i = 0; i < 7; i++) {
-            /*
-            float dbg;
-            bytesToFloat(&dbg, buffer[i]);
-            Serial.println(dbg);
-             */
-            Wire.write(buffer[i], sizeof(float));
-        }
-        Wire.endTransmission();
-    } else {
-        Serial.println("OK");
-        return;
-    }
-
-    if (command.type == CommandType::G01 || command.type == CommandType::G00) {
-        Wire.requestFrom(8, 1);
-        waitingForNext = true;
-        return;
-    }
-
-    if (command.type == CommandType::M99) {
-        Wire.requestFrom(8, 5 * sizeof(float));
-
-        float resp[5];
-        byte buffer[sizeof(float)];
-
-        for (int i = 0; i < 5; i++) {
-            for (int j = 0; j < sizeof(float); j++) {
-                while (!Wire.available()) {
-                }
-                buffer[j] = Wire.read();
-            }
-            bytesToFloat(&resp[i], buffer);
-        }
-
-        Serial.println("Status:");
-        Serial.print("X: ");
-        Serial.println(resp[servoRot]);
-
-        Serial.print("Y: ");
-        Serial.println(resp[eggRot]);
-
-        Serial.print("Xmm: ");
-        Serial.println(resp[servoPos]);
-
-        Serial.print("Ymm: ");
-        Serial.println(resp[eggPos]);
-
-        Serial.print("PEN: ");
-        Serial.println(resp[penPos]);
-
-        return;
-    }
-}
 
 void setup() {
     Serial.begin(115200);
@@ -80,7 +19,7 @@ unsigned long commandTime = 0;
 constexpr unsigned long commandTimeout = 20000;
 
 void loop() {
-    if(millis() - commandTime > commandTimeout) {
+    if (millis() - commandTime > commandTimeout) {
         power.disable12v();
     }
     while (Serial.available() > 0) {
@@ -89,32 +28,23 @@ void loop() {
 
         if (inChar == '\n') {
             inString.trim();
-            if(!power.isEnabled12v()){
+            if (!power.isEnabled12v()) {
                 power.enable12v();
                 delay(100);
             }
-            execCommand(parseGCode(inString));
+            executor.execCommand(parseGCode(inString));
             commandTime = millis();
-            unsigned long reqTime = millis();
+            I2CStatusMsg response;
-            while (waitingForNext) {
+            do {
-                while (!Wire.available()) {
+                response = executor.status();
-                    if (millis() - reqTime > 100) {
+                if (response == I2CStatusMsg::WAIT) {
-                        Wire.requestFrom(8, 1);
-                        reqTime = millis();
-                    }
-                }
-                int response = Wire.read();
-                if (response == WAIT) {
                     delay(1);
-                    Wire.requestFrom(8, 1);
+                } else if (response == I2CStatusMsg::NEXT) {
-                    reqTime = millis();
-                } else if (response == NEXT) {
                     Serial.println("OK");
-                    waitingForNext = false;
                 } else {
                     Serial.println("Error");
                 }
-            }
+            } while (response != I2CStatusMsg::NEXT);
             inString = "";
         }
     }

Firmware/MotorControl/src/main.cpp

@@ -65,7 +65,7 @@ void receiveEvent(int howMany) {
 
 byte txBuffer[5][sizeof(float)];
 void requestEvent() {
-    if (command.type == CommandType::M99) {
+    if (command.type == CommandType::M99 && newCommand) {
         floatToBytes(txBuffer[0], servoStepper.getPos());
         floatToBytes(txBuffer[1], eggStepper.getPos());
 
@@ -74,10 +74,11 @@ void requestEvent() {
 
         floatToBytes(txBuffer[4], (float)pen.getEngaged());
         Wire.write(txBuffer[0], 5 * sizeof(float));
+        newCommand = false;
     } else if (executing || newCommand) {
-        Wire.write(WAIT);
+        Wire.write(static_cast<int>(I2CStatusMsg::WAIT));
     } else {
-        Wire.write(NEXT);
+        Wire.write(static_cast<int>(I2CStatusMsg::NEXT));
     }
 }
 
@@ -85,6 +86,7 @@ void execCommand() {
     executing = true;
 
     if (command.type == CommandType::G01 || command.type == CommandType::G00) {
+        newCommand = false;
         if (command.type == CommandType::G01) {
             needAdjust = true;
         } else {
@@ -109,9 +111,9 @@ void execCommand() {
         }
 
         adjustRPM();
-
-        return;
     }
+
+    return;
 }
 
 void setup() {
@@ -161,7 +163,6 @@ void updateExecution() {
 
 void loop() {
     if (newCommand) {
-        newCommand = false;
         execCommand();
     }
     updateExecution();

Firmware/common/Commands.h

@@ -7,12 +7,12 @@ enum bcAxis {
     Z = 3,
 };
 
-enum I2CMessage {
+enum class I2CStatusMsg {
-    WAIT,
+    WAIT = 0,
     NEXT,
 };
 
-enum StatusMSG {
+enum PosMsg {
     servoRot,
     eggRot,
     servoPos,