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);
+    Status status;
-    while (!Wire.available()) {
+    unsigned int curByte;
-        if (millis() - reqTime > i2cTimeout && tries < i2cTimeoutTries) {
+    byte rxBuffer[i2cStsBytes];
-            Wire.requestFrom(8, 1);
+
-            tries++;
+    Wire.requestFrom(8, i2cStsBytes);
-            reqTime = millis();
+    while (Wire.available() > 0) {
-        } else {
+        char c = Wire.read();
-            return I2CStatusMsg::TIMEOUT;
+        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) {
+    if (cmd.type == LCommandType::ConfList) {
-        for(auto &val : configManager.map) {
+        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) {
+void printSts(Status status) {
-    if (status == I2CStatusMsg::WAIT) {
+    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();

Firmware/MotorControl/include/Config.h

@@ -0,0 +1,6 @@
+#ifndef CONFIG_H
+#define CONFIG_H
+
+constexpr unsigned int stsUpdDelay{10};
+
+#endif

Firmware/MotorControl/src/main.cpp

@@ -2,8 +2,10 @@
 #include <Servo.h>
 #include <Wire.h>
 #include <util/atomic.h>
+#include "Config.h"
 #include "Globals.h"
 #include "common/Commands.h"
+#include "common/Status.h"
 
 int curRPM = DEF_RPM;
 int adjustDelay = 100;
@@ -29,10 +31,13 @@ void adjustRPM() {
             }
         }
     }
-    ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+    int newEggStepperDelay =
-        eggStepperDelay = calculateDelay(eggStepperRPM, STEPS_PER_REVOLUTION);
+        calculateDelay(eggStepperRPM, STEPS_PER_REVOLUTION);
-        servoStepperDelay =
+    int newServoStepperDelay =
         calculateDelay(servoStepperRPM, STEPS_PER_REVOLUTION);
+    ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+        eggStepperDelay = newEggStepperDelay;
+        servoStepperDelay = newServoStepperDelay;
     }
 }
 
@@ -58,24 +63,9 @@ void receiveEvent(int howMany) {
     }
 }
 
-byte txBuffer[5 * i2cFloatSize];
+byte txBuffer[i2cStsBytes];
-void requestEvent() {
+Status sts;
-    if (command.type == CommandType::M99 && newCommand) {
+void requestEvent() { Wire.write(txBuffer, i2cStsBytes); }
-        floatToBytes(&txBuffer[0 * i2cFloatSize], servoStepper.getPos());
-        floatToBytes(&txBuffer[1 * i2cFloatSize], eggStepper.getPos());
-
-        floatToBytes(&txBuffer[2 * i2cFloatSize], servoStepper.getPosMm());
-        floatToBytes(&txBuffer[3 * i2cFloatSize], eggStepper.getPosMm());
-
-        floatToBytes(&txBuffer[4 * i2cFloatSize], (float)pen.getEngaged());
-        Wire.write(txBuffer, 5 * i2cFloatSize);
-        newCommand = false;
-    } else if (executing || newCommand) {
-        Wire.write(static_cast<int>(I2CStatusMsg::WAIT));
-    } else {
-        Wire.write(static_cast<int>(I2CStatusMsg::NEXT));
-    }
-}
 
 void execCommand() {
     executing = true;
@@ -153,6 +143,20 @@ void steppersRoutine() {
         if (tick % servoStepperDelay == 0) {
             servoStepper.doStep();
         }
+        if (tick % stsUpdDelay == 0) {
+            if (executing || newCommand) {
+                sts.type = StatusType::WAIT;
+            } else {
+                sts.type = StatusType::NEXT;
+            }
+
+            sts.mmS = servoStepper.getPosMm();
+            sts.mmE = eggStepper.getPosMm();
+
+            sts.pEng = (float)pen.getEngaged();
+
+            ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { sts.toBytes(txBuffer); }
+        }
         armed = true;
     }
     if (eggStepper.getRemainingSteps() == 0 &&

Firmware/common/Commands.h

@@ -1,8 +1,9 @@
 #ifndef COMMANDS_H
 #define COMMANDS_H
 
-constexpr int i2cFloatSize{4}, i2cCmdFloats{7},
+#include "common/Util.h"
-    i2cCmdBytes{i2cFloatSize * i2cCmdFloats};
+
+constexpr int i2cCmdFloats{7}, i2cCmdBytes{i2cFloatSize * i2cCmdFloats};
 
 enum bcAxis {
     X = 1,
@@ -10,13 +11,6 @@ enum bcAxis {
     Z = 3,
 };
 
-enum class I2CStatusMsg {
-    WAIT = 0,
-    NEXT,
-    TIMEOUT,
-    ERR,
-};
-
 enum PosMsg {
     servoRot,
     eggRot,
@@ -43,14 +37,7 @@ struct Command {
 
     Command(CommandType type = CommandType::unk, float arg1 = NAN,
             float arg2 = NAN, float arg3 = NAN, float arg4 = NAN,
-            float arg5 = NAN, float arg6 = NAN)
+            float arg5 = NAN, float arg6 = NAN);
-        : type(type),
-          arg1(arg1),
-          arg2(arg2),
-          arg3(arg3),
-          arg4(arg4),
-          arg5(arg5),
-          arg6(arg6){};
 
     Command(float *floats);
     void fromFloats(float *floats);
@@ -61,7 +48,4 @@ struct Command {
     void toBytes(byte *bytes);
 };
 
-void bytesToFloat(float *target, byte *val);
-void floatToBytes(byte *target, float val);
-
 #endif

Firmware/common/Status.h

@@ -0,0 +1,39 @@
+#ifndef STATUS_H
+#define STATUS_H
+
+#include "common/Util.h"
+
+constexpr int i2cStsFloats{8}, i2cStsBytes{i2cFloatSize * i2cStsFloats};
+
+enum class StatusType {
+    unk = 0,
+    WAIT,
+    NEXT,
+    TIMEOUT,
+    ERR,
+};
+
+struct Status {
+    StatusType type = StatusType::unk;
+    float mmS = NAN;
+    float mmE = NAN;
+    float pEng = NAN;
+    float feedrate = NAN;
+    float eggDia = NAN;
+    float eggLen = NAN;
+    float xLim = NAN;
+
+    Status(StatusType type = StatusType::unk, float mmS = NAN, float mmE = NAN,
+           float pEng = NAN, float feedrate = NAN, float eggDia = NAN,
+           float eggLen = NAN, float xLim = NAN);
+
+    Status(float *floats);
+    void fromFloats(float *floats);
+    void toFloats(float *floats);
+
+    Status(byte *bytes);
+    void fromBytes(byte *bytes);
+    void toBytes(byte *bytes);
+};
+
+#endif

Firmware/common/Util.h

@@ -0,0 +1,17 @@
+#ifndef UTIL_H
+#define UTIL_H
+
+#include <Arduino.h>
+
+constexpr int i2cFloatSize{4};
+
+inline void bytesToFloat(float *target, byte *val) {
+    memcpy(target, val, i2cFloatSize);
+    return;
+}
+
+inline void floatToBytes(byte *target, float val) {
+    memcpy(target, &val, i2cFloatSize);
+    return;
+}
+#endif

Firmware/commonSrc/Commands.cpp

@@ -2,6 +2,16 @@
 
 #include "common/Commands.h"
 
+Command::Command(CommandType type, float arg1, float arg2, float arg3,
+                 float arg4, float arg5, float arg6)
+    : type(type),
+      arg1(arg1),
+      arg2(arg2),
+      arg3(arg3),
+      arg4(arg4),
+      arg5(arg5),
+      arg6(arg6){};
+
 Command::Command(float *floats) { fromFloats(floats); }
 
 void Command::fromFloats(float *floats) {
@@ -41,13 +51,3 @@ void Command::toBytes(byte *bytes) {
         floatToBytes(&bytes[i * i2cFloatSize], floats[i]);
     }
 }
-
-void bytesToFloat(float *target, byte *val) {
-    memcpy(target, val, i2cFloatSize);
-    return;
-}
-
-void floatToBytes(byte *target, float val) {
-    memcpy(target, &val, i2cFloatSize);
-    return;
-}

Firmware/commonSrc/Status.cpp

@@ -0,0 +1,54 @@
+#include "common/Status.h"
+
+Status::Status(StatusType type, float mmS, float mmE, float pEng,
+               float feedrate, float eggDia, float eggLen, float xLim)
+    : type(type),
+      mmS(mmS),
+      mmE(mmE),
+      pEng(pEng),
+      feedrate(feedrate),
+      eggDia(eggDia),
+      eggLen(eggLen),
+      xLim(xLim){};
+
+Status::Status(float *floats) { fromFloats(floats); }
+
+void Status::fromFloats(float *floats) {
+    type = static_cast<StatusType>(floats[0]);
+    mmS = floats[1];
+    mmE = floats[2];
+    pEng = floats[3];
+    feedrate = floats[4];
+    eggDia = floats[5];
+    eggLen = floats[6];
+    xLim = floats[7];
+}
+
+void Status::toFloats(float *floats) {
+    floats[0] = static_cast<float>(type);
+    floats[1] = mmS;
+    floats[2] = mmE;
+    floats[3] = pEng;
+    floats[4] = feedrate;
+    floats[5] = eggDia;
+    floats[6] = eggLen;
+    floats[7] = xLim;
+}
+
+Status::Status(byte *bytes) { fromBytes(bytes); }
+
+void Status::fromBytes(byte *bytes) {
+    float floats[i2cStsFloats];
+    for (int i = 0; i < i2cStsFloats; i++) {
+        bytesToFloat(&floats[i], &bytes[i * i2cFloatSize]);
+    }
+    fromFloats(floats);
+}
+
+void Status::toBytes(byte *bytes) {
+    float floats[i2cStsFloats];
+    toFloats(floats);
+    for (int i = 0; i < 7; i++) {
+        floatToBytes(&bytes[i * i2cFloatSize], floats[i]);
+    }
+}