From db98b6daabd63f78b347e514c71cfe8d8503267a Mon Sep 17 00:00:00 2001
From: Tim Evers <tim.evers@gmail.com>
Date: Sat, 1 Jul 2017 21:58:42 +0200
Subject: [PATCH] Fix emergency stop

---
 src/PinControl.cpp     | 18 ++++++++
 src/PinControl.h       |  2 +-
 src/StepperControl.cpp | 94 ++++++++++++++++++++++--------------------
 src/StepperControl.h   |  3 ++
 4 files changed, 72 insertions(+), 45 deletions(-)

diff --git a/src/PinControl.cpp b/src/PinControl.cpp
index ec45316..3ade625 100644
--- a/src/PinControl.cpp
+++ b/src/PinControl.cpp
@@ -14,6 +14,12 @@ PinControl *PinControl::getInstance()
 
 PinControl::PinControl()
 {
+  for (int pinNr = 1; pinNr <= 52; pinNr++)
+  {
+    pinWritten[0][pinNr] = false;
+    pinWritten[1][pinNr] = false;
+  }
+
 }
 
 int PinControl::setMode(int pinNr, int mode)
@@ -49,11 +55,23 @@ void PinControl::resetPinsUsed()
   {
     if (pinWritten[0][pinNr])
     {
+      Serial.print("R99");
+      Serial.print(" resetting digital pin");
+      Serial.print(pinNr);
+      Serial.print("\r\n");
+
       digitalWrite(pinNr, false);
+      pinWritten[0][pinNr] = false;
     }
     if (pinWritten[1][pinNr])
     {
+      Serial.print("R99");
+      Serial.print(" resetting analog pin");
+      Serial.print(pinNr);
+      Serial.print("\r\n");
+
       analogWrite(pinNr, 0);
+      pinWritten[1][pinNr] = false;
     }
   }
 }
diff --git a/src/PinControl.h b/src/PinControl.h
index 0ea07fc..788e147 100644
--- a/src/PinControl.h
+++ b/src/PinControl.h
@@ -32,7 +32,7 @@ private:
   PinControl(PinControl const &);
   void operator=(PinControl const &);
 
-  bool pinWritten[1][56];
+  bool pinWritten[2][56];
 
 };
 
diff --git a/src/StepperControl.cpp b/src/StepperControl.cpp
index 2f5d4ba..3579981 100644
--- a/src/StepperControl.cpp
+++ b/src/StepperControl.cpp
@@ -528,7 +528,12 @@ int StepperControl::moveToCoords(long xDest, long yDest, long zDest,
       incomingByte = Serial.read();
       if (incomingByte == 69 || incomingByte == 101)
       {
-        Serial.print("R99 emergency stop\r\n");
+        serialBuffer += "R99 emergency stop\r\n";
+
+        Serial.print(COMM_REPORT_EMERGENCY_STOP);
+        CurrentState::getInstance()->printQAndNewLine();
+
+        //Serial.print("R99 emergency stop\r\n");
 
         emergencyStop = true;
 
@@ -546,7 +551,8 @@ int StepperControl::moveToCoords(long xDest, long yDest, long zDest,
 
     if (error == 1)
     {
-      Serial.print("R99 error\r\n");
+      serialBuffer += "R99 error\r\n";
+      //Serial.print("R99 error\r\n");
 
       axisActive[0] = false;
       axisActive[1] = false;
@@ -554,48 +560,7 @@ int StepperControl::moveToCoords(long xDest, long yDest, long zDest,
     }
 
     // Send the serial buffer one character per cycle to keep motor timing more accuracte
-    if (serialBuffer.length() > 0)
-    {
-
-      //Serial.print("[buf");
-      //Serial.print(serialBufferLength);
-      //Serial.print("]");
-
-      //Serial.print("[send");
-      //Serial.print(serialBufferSending);
-      //Serial.print("]");
-
-      if (serialBufferSending < serialBuffer.length())
-      {
-        //Serial.print("-");
-        switch(serialBuffer.charAt(serialBufferSending))
-        {
-          case 13:
-            Serial.print("\r\n");
-            break;
-          case 10:
-            break;
-          default:
-            Serial.print(serialBuffer.charAt(serialBufferSending));
-            break;
-        }
-
-        //Serial.print("<");
-        //Serial.print(serialBufferSending);
-        //Serial.print(">");
-
-        serialBufferSending++;
-      }
-      else
-      {
-        //Serial.print("reset");
-        // Reset length of buffer when done
-        serialBuffer = "";
-        serialBufferSending = 0;
-        
-        //Serial.println("YY");
-      }
-    }
+    serialBufferSendNext();
 
     // Periodically send message still active
     currentMillis++;
@@ -640,6 +605,7 @@ int StepperControl::moveToCoords(long xDest, long yDest, long zDest,
     }
   }
 
+  serialBufferEmpty();
   Serial.print("R99 stopped\r\n");
 
   // Stop motors
@@ -690,6 +656,46 @@ int StepperControl::moveToCoords(long xDest, long yDest, long zDest,
   return error;
 }
 
+void StepperControl::serialBufferEmpty()
+{
+  while (serialBuffer.length() > 0)
+  {
+    serialBufferSendNext();
+  }
+}
+
+void StepperControl::serialBufferSendNext()
+{
+  // Send the next char in the serialBuffer
+  if (serialBuffer.length() > 0)
+  {
+
+    if (serialBufferSending < serialBuffer.length())
+    {
+      //Serial.print("-");
+      switch (serialBuffer.charAt(serialBufferSending))
+      {
+      case 13:
+        Serial.print("\r\n");
+        break;
+      case 10:
+        break;
+      default:
+        Serial.print(serialBuffer.charAt(serialBufferSending));
+        break;
+      }
+
+      serialBufferSending++;
+    }
+    else
+    {
+      // Reset length of buffer when done
+      serialBuffer = "";
+      serialBufferSending = 0;
+    }
+  }
+}
+
 //
 // Calibration
 //
diff --git a/src/StepperControl.h b/src/StepperControl.h
index 2546d2e..139afa5 100644
--- a/src/StepperControl.h
+++ b/src/StepperControl.h
@@ -78,6 +78,9 @@ private:
   int serialMessageNr = 0;
   int serialMessageDelay = 0;
 
+  void serialBufferSendNext();
+  void serialBufferEmpty();
+
   void checkAxisVsEncoder(StepperControlAxis *axis, StepperControlEncoder *encoder, float *missedSteps, long *lastPosition, long *encoderLastPosition, int *encoderUseForPos, float *encoderStepDecay, bool *encoderEnabled);
   void checkAxisSubStatus(StepperControlAxis *axis, int *axisSubStatus);