From 472011554d1c52bd85bfd2bc5fedc2d6c3836982 Mon Sep 17 00:00:00 2001
From: Stepan Usatiuk <stepanusatiuk@gmail.com>
Date: Mon, 22 Jul 2019 20:02:52 +0300
Subject: [PATCH] clean up loop()

---
 Firmware/EggbotWireless/include/Executor.h |  4 +-
 Firmware/EggbotWireless/src/Executor.cpp   | 26 ++++++++----
 Firmware/EggbotWireless/src/Power.cpp      |  8 ++--
 Firmware/EggbotWireless/src/main.cpp       | 49 ++++++++++++++--------
 Firmware/common/Commands.h                 |  2 +
 5 files changed, 60 insertions(+), 29 deletions(-)

diff --git a/Firmware/EggbotWireless/include/Executor.h b/Firmware/EggbotWireless/include/Executor.h
index 235a059..cd96034 100644
--- a/Firmware/EggbotWireless/include/Executor.h
+++ b/Firmware/EggbotWireless/include/Executor.h
@@ -10,7 +10,9 @@
 class Executor
 {
 private:
-    /* data */
+    unsigned long lastStsTime;
+    I2CStatusMsg lastSts;
+    static constexpr int lastStsTTL = 1;
 public:
     Executor(/* args */);
     void execCommand(Command command);
diff --git a/Firmware/EggbotWireless/src/Executor.cpp b/Firmware/EggbotWireless/src/Executor.cpp
index 636080c..de50c33 100644
--- a/Firmware/EggbotWireless/src/Executor.cpp
+++ b/Firmware/EggbotWireless/src/Executor.cpp
@@ -64,15 +64,27 @@ void Executor::execCommand(Command command) {
 
 I2CStatusMsg Executor::status() {
     unsigned long reqTime = millis();
-    Wire.requestFrom(8, 1);
+    int tries = 0;
 
-    while (!Wire.available()) {
-        if (millis() - reqTime > 100) {
-            Wire.requestFrom(8, 1);
-            reqTime = millis();
-        }
+    if (reqTime - lastStsTime < lastStsTTL) {
+        return lastSts;
     }
 
+    Wire.requestFrom(8, 1);
+    while (!Wire.available()) {
+        if (millis() - reqTime > 10 && tries < 10) {
+            Wire.requestFrom(8, 1);
+            tries++;
+            reqTime = millis();
+        } else {
+            return I2CStatusMsg::TIMEOUT;
+        }
+        delay(1);
+    }
     int resp = Wire.read();
-    return static_cast<I2CStatusMsg>(resp);
+
+    lastStsTime = millis();
+    lastSts = static_cast<I2CStatusMsg>(resp);
+
+    return lastSts;
 }
\ No newline at end of file
diff --git a/Firmware/EggbotWireless/src/Power.cpp b/Firmware/EggbotWireless/src/Power.cpp
index cfbd1f9..4a60772 100644
--- a/Firmware/EggbotWireless/src/Power.cpp
+++ b/Firmware/EggbotWireless/src/Power.cpp
@@ -27,10 +27,12 @@ void Power::enable12v() {
     enabled12v = true;
 }
 
-bool Power::isEnabled12v() {
-    return enabled12v;
-}
+bool Power::isEnabled12v() { return enabled12v; }
 
 void Power::commandHook() {
     lastCmdTime = millis();
+    if (!isEnabled12v()) {
+        enable12v();
+        delay(100);
+    }
 }
\ No newline at end of file
diff --git a/Firmware/EggbotWireless/src/main.cpp b/Firmware/EggbotWireless/src/main.cpp
index fe48f1a..3fc3bee 100644
--- a/Firmware/EggbotWireless/src/main.cpp
+++ b/Firmware/EggbotWireless/src/main.cpp
@@ -2,13 +2,16 @@
 #include <ESP8266WiFi.h>
 #include <Ticker.h>
 #include <Wire.h>
+#include "Executor.h"
 #include "GCodeParser.h"
 #include "Globals.h"
-#include "Executor.h"
 #include "Power.h"
 #include "common/Commands.h"
 
 String inString;
+String commandBuf;
+bool newCommand;
+bool shouldPrintSts;
 
 void setup() {
     Serial.begin(115200);
@@ -16,6 +19,21 @@ void setup() {
     power.enable12v();
 }
 
+void sendCommand(String command) {
+    power.commandHook();
+    executor.execCommand(parseGCode(command));
+    shouldPrintSts = true;
+}
+
+void printSts(I2CStatusMsg status) {
+    if (status == I2CStatusMsg::WAIT) {
+        shouldPrintSts = true;
+    } else if (status == I2CStatusMsg::NEXT) {
+        Serial.println("OK");
+    } else {
+        Serial.println("Error");
+    }
+}
 
 void loop() {
     while (Serial.available() > 0) {
@@ -24,24 +42,19 @@ void loop() {
 
         if (inChar == '\n') {
             inString.trim();
-            if (!power.isEnabled12v()) {
-                power.enable12v();
-                delay(100);
-            }
-            power.commandHook();
-            executor.execCommand(parseGCode(inString));
-            I2CStatusMsg response;
-            do {
-                response = executor.status();
-                if (response == I2CStatusMsg::WAIT) {
-                    delay(1);
-                } else if (response == I2CStatusMsg::NEXT) {
-                    Serial.println("OK");
-                } else {
-                    Serial.println("Error");
-                }
-            } while (response != I2CStatusMsg::NEXT);
+            commandBuf = inString;
             inString = "";
+            newCommand = true;
         }
     }
+
+    I2CStatusMsg status = executor.status();
+    if (shouldPrintSts) {
+        shouldPrintSts = false;
+        printSts(status);
+    }
+    if (newCommand && status == I2CStatusMsg::NEXT) {
+        newCommand = false;
+        sendCommand(commandBuf);
+    }
 }
diff --git a/Firmware/common/Commands.h b/Firmware/common/Commands.h
index ec963fc..10973b3 100644
--- a/Firmware/common/Commands.h
+++ b/Firmware/common/Commands.h
@@ -10,6 +10,8 @@ enum bcAxis {
 enum class I2CStatusMsg {
     WAIT = 0,
     NEXT,
+    TIMEOUT,
+    ERR,
 };
 
 enum PosMsg {