Diff: ArmController.cpp

Revision:

17:0dbcbd8587fd

Parent:

15:4bd10f531cdc

Child:

18:224289104fc0

--- a/ArmController.cpp	Tue Jan 19 20:18:23 2016 +0000
+++ b/ArmController.cpp	Fri Jan 22 01:35:07 2016 +0000
@@ -26,8 +26,7 @@
 
 // try to send some status this many ms
 #define SEND_STATUS_TO          500
-// use slower status send rate if paused
-#define SEND_STATUS_PAUSED_TO   20000
+
 
 // controller polling timer
 Timer IdleTimer;
@@ -59,6 +58,11 @@
         ShowLedBlue();
         MainState = MS_Paused;
     }
+    else if (strncmp(cmd, "alert", 6) == 0)
+    {
+        ShowLedRed();
+        MainState = MS_Error;
+    }
     else if (strncmp(cmd, "resume", 6) == 0)
     {
         ShowLedGreen();
@@ -344,22 +348,17 @@
         // check if time to send status
         if (now >= NextSendMs)
         {
-            // if paused, use longer time out for sending data
-            if (MainState != MS_Paused ||
-                now > NextSendMs + SEND_STATUS_PAUSED_TO)
+            NextSendMs = now + SEND_STATUS_TO;
+            if (!PushMeasurements(partSize, robotArm))
             {
-                NextSendMs = now + SEND_STATUS_TO;
-                if (!PushMeasurements(partSize, robotArm))
+                if (sendAlert)
                 {
-                    if (sendAlert)
-                    {
-                        int partix = robotArm.GetLastErrorPart();
-                        int errCode = robotArm.GetLastError();
-                        printf("Hardware error detected joint %d, code %d \r\n", partix, errCode); 
-                        PushHardwareAlert(partix, errCode);
-                        NeedHwReset = true;
-                        MainState = MS_Error;
-                    }
+                    int partix = robotArm.GetLastErrorPart();
+                    int errCode = robotArm.GetLastError();
+                    printf("Hardware error detected joint %d, code %d \r\n", partix, errCode); 
+                    PushHardwareAlert(partix, errCode);
+                    NeedHwReset = true;
+                    MainState = MS_Error;
                 }
             }
         }