fix pen commands

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

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];

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;
     }
 

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;