Diff: Sequences.cpp

Revision:

7:6723f6887d00

Parent:

5:36916b1c5a06

Child:

8:d98e2dec0f40

diff -r feb0a6311594 -r 6723f6887d00 Sequences.cpp
--- a/Sequences.cpp	Mon Dec 28 17:29:12 2015 +0000
+++ b/Sequences.cpp	Tue Dec 29 23:31:28 2015 +0000
@@ -3,30 +3,95 @@
 #include <ActionBuf.h>
 
 const float UpPos = 180.0f;
-const float rightPos = 90.0f;
+const float RightPos = 90.0f;
+const float NoMove = -1.0f;
 
 
 vector<ActionSequence> UpDownSeq;
 vector<ActionSequence> UpTwistSeq;
+vector<ActionSequence> StartSeq;
+vector<ActionSequence> WaveSeq;
 
 
-void MakeSequences(int partSize, vector<float>& homePositions)
+void MakeSequences(int partSize, vector<float>& startPositions)
 {
     vector<float> upPositions;
-
-    for( int partIx = 0; partIx < partSize; partIx++)
+    vector<float> downPositions;
+    vector<float> homePositions;
+    vector<float> waveUpPositions;
+    vector<float> waveDownPositions;
+    vector<float> rightPositions;
+    
+    for (int partIx = 0; partIx < partSize; partIx++)
     {
         upPositions.push_back(UpPos);
     }
+    
+    for (int partIx = 0; partIx < partSize; partIx++)
+    {
+        rightPositions.push_back(UpPos);
+    }
+    
+    // define normal start position
+    homePositions.push_back(RightPos);
+    if (partSize > 3)
+    {
+        homePositions.push_back(225.0f);
+        homePositions.push_back(270.0f);
+        homePositions.push_back(135.0f);
+        for (int partIx = 4; partIx < partSize; partIx++)
+        {
+            homePositions.push_back(180.0f);
+        }
+    }
+
+    // define down position
+    downPositions.push_back(RightPos);
+    if (partSize > 3)
+    {
+        downPositions.push_back(120.0f);
+        downPositions.push_back(240.0f);
+        downPositions.push_back(150.0f);
+        for (int partIx = 4; partIx < partSize; partIx++)
+        {
+            downPositions.push_back(240.0f);
+        }
+    }
+    
+    // define wave up and wave down to only move last part
+    for (int partIx = 0; partIx < partSize - 1; partIx++)
+    {
+        waveUpPositions.push_back(NoMove);
+        waveDownPositions.push_back(NoMove);
+    }
+    waveUpPositions.push_back(120.0f);
+    waveDownPositions.push_back(210.0f);
 
     // define actions
+    ActionSequence moveStart(SA_SetGoal, homePositions, 1500);
     ActionSequence moveUp(SA_SetGoal, upPositions, 2500);
+    ActionSequence moveDown(SA_SetGoal, downPositions, 2500);
+    ActionSequence waveUp(SA_SetGoal, waveUpPositions, 1500);
+    ActionSequence waveDown(SA_SetGoal, waveDownPositions, 1500);
+    
     ActionSequence report(SA_Status);
     ActionSequence pause2(SA_Delay);
     pause2.SetDelay(2000);
-    ActionSequence moveDown(SA_SetGoal, homePositions, 2500);
     ActionSequence rep(SA_Repeat);  
 
+    // add actions into StartSeq
+    StartSeq.clear();
+    StartSeq.push_back(moveStart);
+    StartSeq.push_back(report);
+    
+    // add actions into WaveSeq
+    WaveSeq.clear();
+    WaveSeq.push_back(waveUp);
+    WaveSeq.push_back(report);
+    WaveSeq.push_back(waveDown);
+    WaveSeq.push_back(report);
+    WaveSeq.push_back(rep);
+    
     // add actions into UpDownSeq
     UpDownSeq.clear();
     UpDownSeq.push_back(moveUp);
@@ -37,25 +102,19 @@
     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++)
+    for (int partIx = 0; partIx < partSize; partIx++)
     {
         vector<float>* rightPositions = new vector<float>();
-        for( int partIy = 0; partIy < partSize; partIy++)
+        for (int partIy = 0; partIy < partSize; partIy++)
         {
             float val = UpPos;
             if (partIx == partIy)
-                val = rightPos;
+                val = RightPos;
             rightPositions->push_back(val);
         }