support for G00 command

Signed-off-by: Stepan Usatyuk <usaatyuk@ustk.me>

Firmware/EggbotWireless/include/GCodeParser.h

@@ -5,6 +5,7 @@
 
 enum command {
     unk,
+    G00,
     G01,
     M99,
 };

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,9 +58,15 @@ float* parseGCode(String gcode) {
                 bytecode[Z] = floatValue;
             }
         }
-
+        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) {
         bytecode[0] = M99;

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,8 +11,14 @@
 
 int RPM = DEF_RPM;
 
+bool needAdjust = false;
+
+Ticker adjustTicker;
+
 void execCommand(float *command) {
+    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]);
@@ -26,6 +34,9 @@ void execCommand(float *command) {
                     }
                 }
             }
+        } else {
+            needAdjust = false;
+        }
 
         if (command[X] != -1) {
             servoStepper.moveTo(command[X]);
@@ -57,25 +68,8 @@ void execCommand(float *command) {
     }
 }
 
-void setup() {
+void adjustRPM() {
-    Serial.begin(115200);
+    if (needAdjust) {
-    eggStepperTimer.setStepper(&eggStepper);
-    servoStepperTimer.setStepper(&servoStepper);
-    pen.init();
-    servoStepper.setPos(70);
-    WiFi.mode(WIFI_OFF);
-}
-
-void loop() {
-    while (Serial.available() > 0) {
-        char inChar = Serial.read();
-        inString += inChar;
-
-        if (inChar == '\n') {
-            inString.trim();
-            execCommand(parseGCode(inString));
-            while (!eggStepper.finished() || !servoStepper.finished()) {
-                delay(50);
         cli();
         int stepsX = servoStepper.getRemainingSteps();
         int stepsY = eggStepper.getRemainingSteps();
@@ -91,6 +85,29 @@ void loop() {
         }
         sei();
     }
+}
+
+void setup() {
+    WiFi.mode(WIFI_OFF);
+    Serial.begin(115200);
+    eggStepperTimer.setStepper(&eggStepper);
+    servoStepperTimer.setStepper(&servoStepper);
+    pen.init();
+    servoStepper.setPos(70);
+    adjustTicker.attach_ms(50, adjustRPM);
+}
+
+void loop() {
+    while (Serial.available() > 0) {
+        char inChar = Serial.read();
+        inString += inChar;
+
+        if (inChar == '\n') {
+            inString.trim();
+            execCommand(parseGCode(inString));
+            while (!eggStepper.finished() || !servoStepper.finished()) {
+                delay(50);
+            }
             eggStepperTimer.setRPM(RPM);
             servoStepperTimer.setRPM(RPM);
             Serial.println("OK");