From a1d59ea6b0ff2d89f0585bf2244c4f87a350dade Mon Sep 17 00:00:00 2001
From: Stepan Usatiuk <stepanusatiuk@gmail.com>
Date: Sat, 18 May 2019 15:27:34 +0300
Subject: [PATCH] fix pen commands

Signed-off-by: Stepan Usatyuk <usaatyuk@ustk.me>
---
 Firmware/EggbotWireless/src/GCodeParser.cpp |  2 +-
 Firmware/EggbotWireless/src/main.cpp        |  4 ++--
 Firmware/MotorControl/src/main.cpp          | 18 ++++++++++--------
 Utils/egg.svg                               |  8 ++------
 Utils/sendcode.py                           |  4 ++--
 5 files changed, 17 insertions(+), 19 deletions(-)

diff --git a/Firmware/EggbotWireless/src/GCodeParser.cpp b/Firmware/EggbotWireless/src/GCodeParser.cpp
index 4f8a48a..1179cc4 100644
--- a/Firmware/EggbotWireless/src/GCodeParser.cpp
+++ b/Firmware/EggbotWireless/src/GCodeParser.cpp
@@ -5,7 +5,7 @@ float bytecode[4] = {-1, -1, -1, -1};
 
 float* parseGCode(String gcode) {
     for (int i = 0; i < 4; i++) {
-        bytecode[i] = -1.0F;
+        bytecode[i] = nanf("");
     }
 
     char commandStringIn[50];
diff --git a/Firmware/EggbotWireless/src/main.cpp b/Firmware/EggbotWireless/src/main.cpp
index e52fd17..153581b 100644
--- a/Firmware/EggbotWireless/src/main.cpp
+++ b/Firmware/EggbotWireless/src/main.cpp
@@ -6,7 +6,7 @@
 #include "common/Commands.h"
 
 String inString;
-bool waitingForNext;
+bool waitingForNext = false;
 
 void sendCommand(float *command) {
     if (command[0] != unk) {
@@ -18,7 +18,7 @@ void sendCommand(float *command) {
         }
         Wire.endTransmission();
     } else {
-        Serial.println("UNK");
+        Serial.println("OK");
         return;
     }
 
diff --git a/Firmware/MotorControl/src/main.cpp b/Firmware/MotorControl/src/main.cpp
index 807d6ae..a524019 100644
--- a/Firmware/MotorControl/src/main.cpp
+++ b/Firmware/MotorControl/src/main.cpp
@@ -84,21 +84,23 @@ void execCommand(float *command) {
             needAdjust = false;
         }
 
-        if (command[X] != -1) {
+        if (!isnan(command[X])) {
             servoStepper.moveTo(command[X]);
         }
 
-        if (command[Y] != -1) {
+        if (!isnan(command[Y])) {
             eggStepper.moveTo(command[Y]);
         }
 
-        if (command[Z] < 0) {
-            pen.engage();
+        if (!isnan(command[Z])) {
+            if (command[Z] < 0) {
+                pen.engage();
+            }
+            if (command[Z] >= 0) {
+                pen.disengage();
+            }
         }
-        if (command[Z] >= 0) {
-            pen.disengage();
-        }
-
+        
         adjustRPM();
 
         return;
diff --git a/Utils/egg.svg b/Utils/egg.svg
index 1de8e60..df02b92 100644
--- a/Utils/egg.svg
+++ b/Utils/egg.svg
@@ -34,8 +34,8 @@
      inkscape:pageopacity="0.0"
      inkscape:pageshadow="2"
      inkscape:zoom="0.7"
-     inkscape:cx="770.76884"
-     inkscape:cy="412.71372"
+     inkscape:cx="661.33551"
+     inkscape:cy="332.89105"
      inkscape:document-units="mm"
      inkscape:current-layer="layer2"
      showgrid="false"
@@ -403,10 +403,6 @@
              x="150">(None)</tspan></text>
       </g>
     </g>
-    <g
-       id="g328" />
-    <g
-       id="g952" />
   </g>
   <g
      inkscape:groupmode="layer"
diff --git a/Utils/sendcode.py b/Utils/sendcode.py
index f2bbb17..0a6ecfe 100644
--- a/Utils/sendcode.py
+++ b/Utils/sendcode.py
@@ -6,14 +6,14 @@ if len(sys.argv) != 3:
     print("Not 2 arguments")
     exit()
 
-gcode = sys.argv[1]
+gcode = sys.argv[2]
 if not os.path.isfile(gcode):
     print("File doesn't exist!")
     exit()
 
 ser = serial.Serial()
 ser.baudrate = 115200
-ser.port = sys.argv[2]
+ser.port = sys.argv[1]
 ser.open()
 
 if not ser.is_open: