Diff: main.cpp

Revision:

5:36916b1c5a06

Parent:

4:36a4eceb1b7f

Child:

7:6723f6887d00

--- a/main.cpp	Wed Dec 23 18:34:06 2015 +0000
+++ b/main.cpp	Mon Dec 28 17:19:37 2015 +0000
@@ -13,6 +13,7 @@
 #include <MeasureBuf.h>
 #include <IothubRobotArm.h>
 #include <ActionBuf.h>
+#include <Sequences.h>
 
 using namespace std;
 
@@ -22,7 +23,7 @@
 extern Terminal* RobotArmPc;
 extern Terminal* AX12Pc;
 
-const float UpPos = 150.0f;
+const float UpPos = 180.0f;
 
 
 void PushMeasurements(MeasureGroup measureGroup, int partSize, RobotArm robotArm)
@@ -129,11 +130,11 @@
 void MakeDemoSeq(vector<ActionSequence>& actions)
 {
     // define actions
-    ActionSequence moveUp(SA_SetGoal, upPositions, 3000);
+    ActionSequence moveUp(SA_SetGoal, upPositions, 2500);
     ActionSequence report(SA_Status);
     ActionSequence pause(SA_Delay);
-    pause.SetDelay(5000);
-    ActionSequence moveDown(SA_SetGoal, homePositions, 3000);
+    pause.SetDelay(2000);
+    ActionSequence moveDown(SA_SetGoal, homePositions, 2500);
     ActionSequence rep(SA_Repeat);  
 
     // add actions into a sequence
@@ -160,12 +161,51 @@
 };
 
 Queue<vector<ActionSequence>, 16> SequenceQ;
-MainStates MainState;
+volatile MainStates MainState;
 bool RunInSequence;
 bool RunInMoveStep;
 bool RunInDelay;
 void* CancelToMatch;
 
+osThreadId mainTid;
+
+void ControlArm(const char* cmd)
+{
+    if (strncmp(cmd, "pause", 5) == 0)
+    {
+        pc.printf( "Pause cmd\r\n"); 
+        MainState = MS_Paused;
+        osSignalSet(mainTid, AS_Action);
+    }
+    else if (strncmp(cmd, "resume", 6) == 0)
+    {
+        pc.printf( "Resume cmd\r\n"); 
+        MainState = MS_Running;
+        osSignalSet(mainTid, AS_Action);
+    }
+    else if (strncmp(cmd, "runupdown", 9) == 0)
+    {
+        pc.printf( "runupdown cmd\r\n"); 
+        MainState = MS_CancelToM;
+        CancelToMatch = &UpDownSeq;
+        SequenceQ.put(&UpDownSeq);
+        osSignalSet(mainTid, AS_Action);
+    }
+    else if (strncmp(cmd, "runuptwist", 10) == 0)
+    {
+        pc.printf( "runuptwist cmd\r\n"); 
+        MainState = MS_CancelOne;
+        CancelToMatch = &UpTwistSeq;
+        SequenceQ.put(&UpTwistSeq);
+        osSignalSet(mainTid, AS_Action);
+    }
+    else if (strncmp(cmd, "cancelone", 9) == 0)
+    {
+        pc.printf( "CancelOne cmd\r\n"); 
+        MainState = MS_CancelOne;
+        osSignalSet(mainTid, AS_Action);
+    }
+}
 
 // run sequence thread timer routine
 void NextSeq(void const * tid)
@@ -203,15 +243,14 @@
     SendIothubMeasurements();
  
     // Prepare main thread
-    osThreadId mainTid = osThreadGetId();
+    mainTid = osThreadGetId();
     int32_t signals = AS_Action;
     
-    // create a sequence for demo
-    vector<ActionSequence> actions;
-    MakeDemoSeq(actions);
-    
+    // create sequences
+    MakeSequences(partSize, homePositions);
+  
     // add to queue
-    SequenceQ.put(&actions);
+    SequenceQ.put(&UpDownSeq);
     
     // state for sequence
     vector<ActionSequence> *curseq = NULL;
@@ -345,9 +384,15 @@
                     if (!RunInMoveStep)
                     {
                         pc.printf( "Next Seq %d\r\n", curseqIx); 
-            
+                        
                         if (curseq != NULL)
                         {
+                            if (curseqIx >= curseq->size())
+                            {
+                                RunInSequence = false;
+                                break;
+                            }
+
                             ActionSequence aseq = (*curseq)[curseqIx];
                             curseqIx++;
                             
@@ -391,11 +436,6 @@
                                     
                             }
                         }
-                        
-                        if (curseqIx >= curseq->size())
-                        {
-                            RunInSequence = false;
-                        }
                     }
  
                 }