status now always reports position

Firmware/EggbotWireless/include/Config.h

@@ -3,7 +3,7 @@
 
 constexpr int pin12v{5};
 constexpr int i2cTimeout{10}, i2cTimeoutTries{10};
-constexpr int lastStsTTL{1};
+constexpr int lastStsTTL{10};
 constexpr unsigned long powerTimeout{20000}, powerStartupDelay{100};
 
 constexpr const char *defSSID{"eggbot"}, *defPass{"eggbotwi"};

Firmware/EggbotWireless/include/Executor.h

@@ -5,17 +5,17 @@
 #include <Wire.h>
 
 #include "common/Commands.h"
-
+#include "common/Status.h"
 
 class Executor
 {
 private:
     unsigned long lastStsTime;
-    I2CStatusMsg lastSts;
+    Status lastSts;
 public:
     Executor(/* args */);
     void execCommand(Command command);
-    I2CStatusMsg status();
+    Status status();
 };
 
 extern Executor executor;

Firmware/EggbotWireless/include/LocalCommand.h

@@ -11,6 +11,7 @@ enum class LCommandType {
     ConfLoad,   // LCL
     ConfWrite,  // LCW
     ConfReset,  // LCR
+    StsPrint,   // LSP
 };
 
 struct LCommand {

Firmware/EggbotWireless/src/Executor.cpp

@@ -1,6 +1,7 @@
 #include "Executor.h"
 #include "Config.h"
 #include "common/Commands.h"
+#include "common/Util.h"
 
 Executor executor;
 
@@ -8,79 +9,44 @@ Executor::Executor() {}
 
 void Executor::execCommand(Command command) {
     if (command.type == CommandType::unk) {
-        Serial.println("OK");
         return;
     }
 
-    Wire.beginTransmission(8);
     byte buffer[i2cCmdBytes];
     command.toBytes(buffer);
+    Wire.beginTransmission(8);
     Wire.write(buffer, i2cCmdBytes);
     Wire.endTransmission();
 
     if (command.type == CommandType::G01 || command.type == CommandType::G00) {
         return;
     }
-
-    if (command.type == CommandType::M99) {
-        delay(10);
-        Wire.requestFrom(8, 5 * i2cFloatSize);
-
-        float resp[5];
-        byte buffer[i2cFloatSize];
-
-        for (int i = 0; i < 5; i++) {
-            for (unsigned int j = 0; j < i2cFloatSize; 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() {
+Status Executor::status() {
     unsigned long reqTime = millis();
-    int tries = 0;
 
     if (reqTime - lastStsTime < lastStsTTL) {
         return lastSts;
     }
 
-    Wire.requestFrom(8, 1);
-    while (!Wire.available()) {
-        if (millis() - reqTime > i2cTimeout && tries < i2cTimeoutTries) {
-            Wire.requestFrom(8, 1);
-            tries++;
-            reqTime = millis();
-        } else {
-            return I2CStatusMsg::TIMEOUT;
+    Status status;
+    unsigned int curByte;
+    byte rxBuffer[i2cStsBytes];
+
+    Wire.requestFrom(8, i2cStsBytes);
+    while (Wire.available() > 0) {
+        char c = Wire.read();
+        rxBuffer[curByte] = c;
+        curByte++;
+        if (curByte == i2cStsBytes) {
+            curByte = 0;
+            status.fromBytes(rxBuffer);
         }
-        delay(1);
     }
-    int resp = Wire.read();
 
     lastStsTime = millis();
-    lastSts = static_cast<I2CStatusMsg>(resp);
+    lastSts = status;
 
-    return lastSts;
+    return status;
 }

Firmware/EggbotWireless/src/LocalCommand.cpp

@@ -27,6 +27,8 @@ void LCommand::fromChars(char *cmd) {
                 type = LCommandType::ConfWrite;
             } else if (strcmp("LCR", cmd) == 0) {
                 type = LCommandType::ConfReset;
+            } else if (strcmp("LSP", cmd) == 0) {
+                type = LCommandType::StsPrint;
             }
         } else if (i == 1) {
             strncpy(arg1, token, 25);

Firmware/EggbotWireless/src/LocalExecutor.cpp

@@ -2,6 +2,8 @@
 
 #include <string>
 #include "ConfigManager.h"
+#include "Executor.h"
+#include "common/Status.h"
 
 LocalExecutor::LocalExecutor() {}
 
@@ -26,20 +28,20 @@ void LocalExecutor::execCommand(LCommand cmd) {
         return;
     }
 
-    if(cmd.type == LCommandType::ConfList) {
-        for(auto &val : configManager.map) {
+    if (cmd.type == LCommandType::ConfList) {
+        for (auto &val : configManager.map) {
             Serial.print(val.first.c_str());
             Serial.print(": ");
             Serial.println(val.second.c_str());
         }
     }
 
-    if(cmd.type == LCommandType::ConfLoad) {
+    if (cmd.type == LCommandType::ConfLoad) {
         configManager.load();
         return;
     }
 
-    if(cmd.type == LCommandType::ConfWrite) {
+    if (cmd.type == LCommandType::ConfWrite) {
         configManager.write();
         return;
     }
@@ -48,6 +50,22 @@ void LocalExecutor::execCommand(LCommand cmd) {
         configManager.reset();
         return;
     }
+
+    if (cmd.type == LCommandType::StsPrint) {
+        Status status = executor.status();
+
+        Serial.println("Status:");
+
+        Serial.print("Xmm: ");
+        Serial.println(status.mmS);
+
+        Serial.print("Ymm: ");
+        Serial.println(status.mmE);
+
+        Serial.print("PEN: ");
+        Serial.println(status.pEng);
+        return;
+    }
 }
 
 LocalExecutor localExecutor;

Firmware/EggbotWireless/src/main.cpp

@@ -9,8 +9,8 @@
 #include "GCodeParser.h"
 #include "Globals.h"
 #include "LocalExecutor.h"
-#include "WiFiManager.h"
 #include "Power.h"
+#include "WiFiManager.h"
 #include "common/Commands.h"
 
 std::queue<Command> commandQueue;
@@ -25,13 +25,16 @@ void setup() {
     wifiManager.init();
 }
 
-void printSts(I2CStatusMsg status) {
-    if (status == I2CStatusMsg::WAIT) {
+void printSts(Status status) {
+    if (status.type == StatusType::WAIT) {
         shouldPrintSts = true;
-    } else if (status == I2CStatusMsg::NEXT) {
+    } else if (status.type == StatusType::NEXT) {
         Serial.println("OK");
+    } else if (status.type == StatusType::TIMEOUT) {
+        Serial.println("Timeout");
     } else {
-        Serial.println("Error");
+        Serial.print("Error: ");
+        Serial.println(static_cast<int>(status.type));
     }
 }
 
@@ -65,12 +68,12 @@ void serialLoop() {
 }
 
 void commandsLoop() {
-    I2CStatusMsg status = executor.status();
+    Status status = executor.status();
     if (shouldPrintSts) {
         shouldPrintSts = false;
         printSts(status);
     }
-    if (status == I2CStatusMsg::NEXT && !commandQueue.empty()) {
+    if (status.type == StatusType::NEXT && !commandQueue.empty()) {
         power.commandHook();
         executor.execCommand(commandQueue.front());
         commandQueue.pop();