From c4d76632a02ccea6e2d51d93245e029d1dc49516 Mon Sep 17 00:00:00 2001
From: Stepan Usatiuk <stepanusatiuk@gmail.com>
Date: Fri, 10 May 2019 16:13:34 +0300
Subject: [PATCH] support for G00 command

Signed-off-by: Stepan Usatyuk <usaatyuk@ustk.me>
---
 Firmware/EggbotWireless/include/GCodeParser.h |  1 +
 Firmware/EggbotWireless/src/GCodeParser.cpp   | 12 ++-
 Firmware/EggbotWireless/src/main.cpp          | 75 ++++++++++++-------
 3 files changed, 55 insertions(+), 33 deletions(-)

diff --git a/Firmware/EggbotWireless/include/GCodeParser.h b/Firmware/EggbotWireless/include/GCodeParser.h
index acbc46e..99acb49 100644
--- a/Firmware/EggbotWireless/include/GCodeParser.h
+++ b/Firmware/EggbotWireless/include/GCodeParser.h
@@ -5,6 +5,7 @@
 
 enum command {
     unk,
+    G00,
     G01,
     M99,
 };
diff --git a/Firmware/EggbotWireless/src/GCodeParser.cpp b/Firmware/EggbotWireless/src/GCodeParser.cpp
index e0d7130..f2a44bd 100644
--- a/Firmware/EggbotWireless/src/GCodeParser.cpp
+++ b/Firmware/EggbotWireless/src/GCodeParser.cpp
@@ -23,8 +23,6 @@ float* parseGCode(String gcode) {
     strncpy(args, &commandString[4], 45);
 
     if (strcmp(command, "G01") == 0 || strcmp(command, "G00") == 0) {
-        bytecode[0] = G01;
-
         char split_args[3][40];
         memset(split_args, 0, sizeof(split_args));
         char* arg;
@@ -60,8 +58,14 @@ float* parseGCode(String gcode) {
                 bytecode[Z] = floatValue;
             }
         }
-
-        return bytecode;
+        if (strcmp(command, "G00") == 0) {
+            bytecode[0] = G00;
+            return bytecode;
+        }
+        if (strcmp(command, "G01") == 0) {
+            bytecode[0] = G01;
+            return bytecode;
+        }
     }
 
     if (strcmp(command, "M99") == 0) {
diff --git a/Firmware/EggbotWireless/src/main.cpp b/Firmware/EggbotWireless/src/main.cpp
index f30b87b..c0ec93a 100644
--- a/Firmware/EggbotWireless/src/main.cpp
+++ b/Firmware/EggbotWireless/src/main.cpp
@@ -1,5 +1,7 @@
 #include <Arduino.h>
 #include <ESP8266WiFi.h>
+#include <Ticker.h>
+
 #include "EggConstants.h"
 #include "GCodeParser.h"
 #include "Globals.h"
@@ -9,22 +11,31 @@
 
 int RPM = DEF_RPM;
 
+bool needAdjust = false;
+
+Ticker adjustTicker;
+
 void execCommand(float *command) {
-    if (command[0] == G01) {
-        if (command[X] != -1 && command[Y] != -1) {
-            float distX = servoStepper.getDistMm(command[X]);
-            float distY = eggStepper.getDistMm(command[Y]);
-            int stepsX = servoStepper.mmToSteps(distX);
-            int stepsY = eggStepper.mmToSteps(distY);
-            if (stepsX != 0 && stepsY != 0) {
-                if (stepsX > stepsY) {
-                    float rpm = (float)RPM * (float)stepsY / (float)stepsX;
-                    eggStepperTimer.setRPM(rpm);
-                } else if (stepsY > stepsX) {
-                    float rpm = (float)RPM * (float)stepsX / (float)stepsY;
-                    servoStepperTimer.setRPM(rpm);
+    if (command[0] == G01 || command[0] == G00) {
+        if (command[0] == G01) {
+            needAdjust = true;
+            if (command[X] != -1 && command[Y] != -1) {
+                float distX = servoStepper.getDistMm(command[X]);
+                float distY = eggStepper.getDistMm(command[Y]);
+                int stepsX = servoStepper.mmToSteps(distX);
+                int stepsY = eggStepper.mmToSteps(distY);
+                if (stepsX != 0 && stepsY != 0) {
+                    if (stepsX > stepsY) {
+                        float rpm = (float)RPM * (float)stepsY / (float)stepsX;
+                        eggStepperTimer.setRPM(rpm);
+                    } else if (stepsY > stepsX) {
+                        float rpm = (float)RPM * (float)stepsX / (float)stepsY;
+                        servoStepperTimer.setRPM(rpm);
+                    }
                 }
             }
+        } else {
+            needAdjust = false;
         }
 
         if (command[X] != -1) {
@@ -57,13 +68,33 @@ void execCommand(float *command) {
     }
 }
 
+void adjustRPM() {
+    if (needAdjust) {
+        cli();
+        int stepsX = servoStepper.getRemainingSteps();
+        int stepsY = eggStepper.getRemainingSteps();
+        if (stepsX > stepsY) {
+            float rpm = (float)RPM * (float)stepsY / (float)stepsX;
+            eggStepperTimer.setRPM(rpm);
+        } else if (stepsY > stepsX) {
+            float rpm = (float)RPM * (float)stepsX / (float)stepsY;
+            servoStepperTimer.setRPM(rpm);
+        } else {
+            eggStepperTimer.setRPM(RPM);
+            servoStepperTimer.setRPM(RPM);
+        }
+        sei();
+    }
+}
+
 void setup() {
+    WiFi.mode(WIFI_OFF);
     Serial.begin(115200);
     eggStepperTimer.setStepper(&eggStepper);
     servoStepperTimer.setStepper(&servoStepper);
     pen.init();
     servoStepper.setPos(70);
-    WiFi.mode(WIFI_OFF);
+    adjustTicker.attach_ms(50, adjustRPM);
 }
 
 void loop() {
@@ -76,20 +107,6 @@ void loop() {
             execCommand(parseGCode(inString));
             while (!eggStepper.finished() || !servoStepper.finished()) {
                 delay(50);
-                cli();
-                int stepsX = servoStepper.getRemainingSteps();
-                int stepsY = eggStepper.getRemainingSteps();
-                if (stepsX > stepsY) {
-                    float rpm = (float)RPM * (float)stepsY / (float)stepsX;
-                    eggStepperTimer.setRPM(rpm);
-                } else if (stepsY > stepsX) {
-                    float rpm = (float)RPM * (float)stepsX / (float)stepsY;
-                    servoStepperTimer.setRPM(rpm);
-                } else {
-                    eggStepperTimer.setRPM(RPM);
-                    servoStepperTimer.setRPM(RPM);
-                }
-                sei();
             }
             eggStepperTimer.setRPM(RPM);
             servoStepperTimer.setRPM(RPM);
@@ -97,4 +114,4 @@ void loop() {
             inString = "";
         }
     }
-}
\ No newline at end of file
+}