Diff: Sequences.cpp

Revision:

5:36916b1c5a06

Child:

7:6723f6887d00

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sequences.cpp	Mon Dec 28 17:19:37 2015 +0000
@@ -0,0 +1,71 @@
+#include "mbed.h"
+#include <vector>
+#include <ActionBuf.h>
+
+const float UpPos = 180.0f;
+const float rightPos = 90.0f;
+
+
+vector<ActionSequence> UpDownSeq;
+vector<ActionSequence> UpTwistSeq;
+
+
+void MakeSequences(int partSize, vector<float>& homePositions)
+{
+    vector<float> upPositions;
+
+    for( int partIx = 0; partIx < partSize; partIx++)
+    {
+        upPositions.push_back(UpPos);
+    }
+
+    // define actions
+    ActionSequence moveUp(SA_SetGoal, upPositions, 2500);
+    ActionSequence report(SA_Status);
+    ActionSequence pause2(SA_Delay);
+    pause2.SetDelay(2000);
+    ActionSequence moveDown(SA_SetGoal, homePositions, 2500);
+    ActionSequence rep(SA_Repeat);  
+
+    // add actions into UpDownSeq
+    UpDownSeq.clear();
+    UpDownSeq.push_back(moveUp);
+    UpDownSeq.push_back(report);
+    UpDownSeq.push_back(pause2);
+    UpDownSeq.push_back(moveDown);
+    UpDownSeq.push_back(report);
+    UpDownSeq.push_back(pause2);
+    UpDownSeq.push_back(rep);
+    
+    vector<float> rightPositions;
+    for( int partIx = 0; partIx < partSize; partIx++)
+    {
+        rightPositions.push_back(UpPos);
+    }
+    
+    // add actions into RightStepsSeq
+    // move to vertical, then move each part one at a time, then return to home
+    UpTwistSeq.clear();
+    UpTwistSeq.push_back(moveUp);
+    
+    for( int partIx = 0; partIx < partSize; partIx++)
+    {
+        vector<float>* rightPositions = new vector<float>();
+        for( int partIy = 0; partIy < partSize; partIy++)
+        {
+            float val = UpPos;
+            if (partIx == partIy)
+                val = rightPos;
+            rightPositions->push_back(val);
+        }
+
+        ActionSequence* seq = new ActionSequence(SA_SetGoal, *rightPositions, 1000);
+        UpTwistSeq.push_back(*seq);
+        UpTwistSeq.push_back(pause2);
+        UpTwistSeq.push_back(report);
+        UpTwistSeq.push_back(moveUp);
+    }
+    UpTwistSeq.push_back(pause2);
+    UpTwistSeq.push_back(moveDown);
+    
+}