Send feedback on invalid command, emergency stop after movement failed after retry

src/Config.h

@@ -21,6 +21,8 @@ const char COMM_REPORT_CMD_BUSY[4] = {'R', '0', '4', '\0'};
 const char COMM_REPORT_CMD_STATUS[4] = {'R', '0', '5', '\0'};
 const char COMM_REPORT_CALIB_STATUS[4] = {'R', '0', '6', '\0'};
 const char COMM_REPORT_CMD_RETRY[4] = { 'R', '0', '7', '\0' };
+const char COMM_REPORT_CMD_ECHO[4] = { 'R', '0', '8', '\0' };
+const char COMM_REPORT_BAD_CMD[4] = { 'R', '0', '9', '\0' };
 
 const char COMM_REPORT_ENCODER_SCALED[4] = { 'R', '8', '4', '\0' };
 const char COMM_REPORT_ENCODER_RAW[4] = { 'R', '8', '5', '\0' };

src/GCodeProcessor.cpp

@@ -100,6 +100,10 @@ int GCodeProcessor::execute(Command *command)
 
   if (command == NULL)
   {
+
+    Serial.print(COMM_REPORT_BAD_CMD);
+    CurrentState::getInstance()->printQAndNewLine();
+
     if (LOGGING)
     {
       printCommandLog(command);
@@ -109,6 +113,9 @@ int GCodeProcessor::execute(Command *command)
 
   if (command->getCodeEnum() == CODE_UNDEFINED)
   {
+    Serial.print(COMM_REPORT_BAD_CMD);
+    CurrentState::getInstance()->printQAndNewLine();
+
     if (LOGGING)
     {
       printCommandLog(command);
@@ -122,6 +129,9 @@ int GCodeProcessor::execute(Command *command)
 
   if (handler == NULL)
   {
+    Serial.print(COMM_REPORT_BAD_CMD);
+    CurrentState::getInstance()->printQAndNewLine();
+
     Serial.println("R99 handler == NULL\r\n");
     return -1;
   }
@@ -135,12 +145,15 @@ int GCodeProcessor::execute(Command *command)
   CurrentState::getInstance()->setLastError(0);
   while (attempt < 1 || (attempt < maximumAttempts && execution != 0))
   {
-    Serial.print("R99 attempt ");
-    Serial.print(attempt);
-    Serial.print(" from ");
-    Serial.print(maximumAttempts);
 
-    Serial.print("\r\n");
+    if (LOGGING || debugMessages)
+    {
+      Serial.print("R99 attempt ");
+      Serial.print(attempt);
+      Serial.print(" from ");
+      Serial.print(maximumAttempts);
+      Serial.print("\r\n");
+    }
 
     attempt++;
     if (attempt > 1)
@@ -152,17 +165,34 @@ int GCodeProcessor::execute(Command *command)
     handler->execute(command);
     execution = CurrentState::getInstance()->getLastError();
 
-    Serial.print("R99 execution ");
-    Serial.print(execution);
-    Serial.print("\r\n");
-
+    if (LOGGING || debugMessages)
+    {
+      Serial.print("R99 execution ");
+      Serial.print(execution);
+      Serial.print("\r\n");
+    }
   }
+
   // Clean serial buffer
   while (Serial.available() > 0)
   {
     Serial.read();
   }
 
+  // if movemement failed after retry
+  // and parameter for emergency stop is set
+  // set the emergency stop
+  if (execution == 0)
+  {
+    if (isMovement)
+    {
+      if (ParameterList::getInstance()->getValue(PARAM_E_STOP_ON_MOV_ERR) == 1)
+      {
+        CurrentState::getInstance()->setEmergencyStop();
+      }
+    }
+  }
+
   // Report back result of execution
   if (execution == 0)
   {

src/GCodeProcessor.h

@@ -11,6 +11,8 @@
 #include "Command.h"
 #include "Config.h"
 
+#include "Debug.h"
+
 #include "GCodeHandler.h"
 #include "G00Handler.h"
 #include "G28Handler.h"

src/farmbot_arduino_controller.cpp

@@ -344,6 +344,11 @@ void loop()
         //Command* command = new Command(commandString);
         Command *command = new Command(commandChar);
 
+        // Report back the received command
+        Serial.print(COMM_REPORT_CMD_ECHO);
+        Serial.print(commandChar);
+
+        // Log the values if needed for debugging
         if (LOGGING || debugMessages)
         {
           command->print();