Diff: ArmController.cpp

Revision:

11:3a2e6eb9fbb8

Parent:

10:9b21566a5ddb

Child:

13:ffeff9b5e513

--- a/ArmController.cpp	Wed Jan 06 00:58:41 2016 +0000
+++ b/ArmController.cpp	Wed Jan 06 22:25:51 2016 +0000
@@ -40,7 +40,8 @@
 int LastSendMs = 0;
 
 Queue<vector<ActionSequence>, 16> SequenceQ;
-volatile MainStates MainState;
+MainStates MainState;
+bool ErrorReset;
 bool RunInSequence;
 bool RunInMoveStep;
 bool RunInDelay;
@@ -175,6 +176,11 @@
 
 void ControlArm(const char* cmd)
 {
+    if (MainState == MS_Error)
+    {
+        ErrorReset = true;
+    }
+    
     if (strncmp(cmd, "pause", 5) == 0)
     {
         ShowLedBlue();
@@ -196,42 +202,42 @@
     else if (strncmp(cmd, "runupdown", 9) == 0)
     {
         ShowLedGreen();
-        MainState = MS_Running;
+        MainState = MS_CancelOne;
         SequenceQ.put(&UpDownSeq);
         osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "runuptwist", 10) == 0)
     {
         ShowLedGreen();
-        MainState = MS_Running;
+        MainState = MS_CancelOne;
         SequenceQ.put(&UpTwistSeq);
         osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "runhome", 10) == 0)
     {
         ShowLedGreen();
-        MainState = MS_Running;
+        MainState = MS_CancelOne;
         SequenceQ.put(&StartSeq);
         osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "runwave", 10) == 0)
     {
         ShowLedGreen();
-        MainState = MS_Running;
+        MainState = MS_CancelOne;
         SequenceQ.put(&WaveSeq);
         osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "runtaps1", 10) == 0)
     {
         ShowLedGreen();
-        MainState = MS_Running;
+        MainState = MS_CancelOne;
         SequenceQ.put(&TapsSeq);
         osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "fastwave", 8) == 0)
     {
         ShowLedGreen();
-        MainState = MS_Running;
+        MainState = MS_CancelOne;
         SequenceQ.put(&FastWaveSeq);
         osSignalSet(mainTid, AS_Action);
     }
@@ -266,7 +272,8 @@
     RunInSequence = false;
     RunInMoveStep = false;
     RunInDelay = false;
-     
+    ErrorReset = false;
+    
     // get initial positions
     MeasureGroup measureGroup;
     robotArm.GetArmPositions(initPositions);
@@ -305,6 +312,12 @@
     {
         osEvent mainEvent = osSignalWait(signals, osWaitForever);
         
+        if (ErrorReset)
+        {
+            ErrorReset = false;
+            robotArm.ClearErrorState();
+        }
+        
         switch (MainState)
         {
             case MS_Idle: