Diff: Sequences.cpp

Revision:

13:ffeff9b5e513

Parent:

11:3a2e6eb9fbb8

Child:

18:224289104fc0

--- a/Sequences.cpp	Thu Jan 07 17:31:23 2016 +0000
+++ b/Sequences.cpp	Fri Jan 15 22:02:46 2016 +0000
@@ -6,101 +6,85 @@
 const float RightPos = 90.0f;
 const float NoMove = -1.0f;
 
-// this must have same value as in RobotArm
-#define NUMPARTS    5
+
+vector<ActionSequence*> UpDownSeq;
+vector<ActionSequence*> UpTwistSeq;
+vector<ActionSequence*> StartSeq;
+vector<ActionSequence*> WaveSeq;
+vector<ActionSequence*> TapsSeq;
+vector<ActionSequence*> BigWaveSeq;
 
-vector<ActionSequence> UpDownSeq;
-vector<ActionSequence> UpTwistSeq;
-vector<ActionSequence> StartSeq;
-vector<ActionSequence> WaveSeq;
-vector<ActionSequence> TapsSeq;
-vector<ActionSequence> FastWaveSeq;
+const float upPositions[] = { UpPos, UpPos, UpPos, UpPos, UpPos };
+const float downPositions[] = { RightPos, 120.0f, 240.0f, 150.0f, 240.0f };
+const float homePositions[] = { RightPos, 225.0f, 270.0f, 135.0f, UpPos };
+const float waveUpPositions[] = { NoMove, NoMove, NoMove, NoMove, 135.0f };
+const float waveDownPositions[] = { NoMove, NoMove, NoMove, NoMove, 225.0f };
+const float waveMiddlePositions[] = { NoMove, NoMove, NoMove, NoMove, 180.0f };
+const float bigwaveUpPositions[] = { NoMove, 135.0f, NoMove, NoMove, NoMove };
+const float bigwaveDownPositions[] = { NoMove, 225.0f, NoMove, NoMove, NoMove };
+    
+const float tapStartPositions[] = { RightPos, 180.0f, 270.0f, 180.0f, 180.0f };
+const float tap1Positions[] = { 60.2f, NoMove, NoMove, NoMove, NoMove };
+const float tap2Positions[] = { 68.7f, NoMove, NoMove, NoMove, NoMove };
+const float tap3Positions[] = { 77.2f, NoMove, NoMove, NoMove, NoMove };
+const float tap4Positions[] = { 85.7f, NoMove, NoMove, NoMove, NoMove };
+const float tap5Positions[] = { 94.2f, NoMove, NoMove, NoMove, NoMove };
+const float tap6Positions[] = { 102.7f, NoMove, NoMove, NoMove, NoMove };
+const float tap7Positions[] = { 111.2f, NoMove, NoMove, NoMove, NoMove };
+const float tap8Positions[] = { 119.7f, NoMove, NoMove, NoMove, NoMove };
+    
 
-void MakeSequences(int partSize, vector<float>& startPositions)
+void MakeSequences(int partSize)
 {
-    float upPositions[] = { UpPos, UpPos, UpPos, UpPos, UpPos };
-    float downPositions[] = { RightPos, 120.0f, 240.0f, 150.0f, 240.0f };
-    float homePositions[] = { RightPos, 225.0f, 270.0f, 135.0f, UpPos };
-    float rightPositions[] = { UpPos, UpPos, UpPos, UpPos, UpPos };
-    float waveUpPositions[] = { NoMove, NoMove, NoMove, NoMove, 135.0f };
-    float waveDownPositions[] = { NoMove, NoMove, NoMove, NoMove, 225.0f };
-    float waveMiddlePositions[] = { NoMove, NoMove, NoMove, NoMove, 180.0f };
-    float fastwaveUpPositions[] = { NoMove, 135.0f, NoMove, NoMove, NoMove };
-    float fastwaveDownPositions[] = { NoMove, 225.0f, NoMove, NoMove, NoMove };
-    
-    float tapStartPositions[] = { RightPos, 180.0f, 270.0f, 180.0f, 180.0f };
-    float tap1Positions[] = { 60.2f, NoMove, NoMove, NoMove, NoMove };
-    float tap2Positions[] = { 68.7f, NoMove, NoMove, NoMove, NoMove };
-    float tap3Positions[] = { 77.2f, NoMove, NoMove, NoMove, NoMove };
-    float tap4Positions[] = { 85.7f, NoMove, NoMove, NoMove, NoMove };
-    float tap5Positions[] = { 94.2f, NoMove, NoMove, NoMove, NoMove };
-    float tap6Positions[] = { 102.7f, NoMove, NoMove, NoMove, NoMove };
-    float tap7Positions[] = { 111.2f, NoMove, NoMove, NoMove, NoMove };
-    float tap8Positions[] = { 119.7f, NoMove, NoMove, NoMove, NoMove };
-    
     // define actions
-    ActionSequence moveStart(SA_SetGoal, partSize, homePositions, 1500);
-    ActionSequence moveUp(SA_SetGoal, partSize, upPositions, 1000);
-    ActionSequence moveDown(SA_SetGoal, partSize, downPositions, 1000);
-    ActionSequence waveUp(SA_SetGoal, partSize, waveUpPositions, 1000);
-    ActionSequence waveDown(SA_SetGoal, partSize, waveDownPositions, 1000);
-    ActionSequence fastwaveUp(SA_SetGoal, partSize, fastwaveUpPositions, 400);
-    ActionSequence fastwaveDown(SA_SetGoal, partSize, fastwaveDownPositions, 400);
-    ActionSequence tapsStart(SA_SetGoal, partSize, tapStartPositions, 1000);
-    ActionSequence taps1(SA_SetGoal, partSize, tap1Positions, 200);
-    ActionSequence taps2(SA_SetGoal, partSize, tap2Positions, 200);
-    ActionSequence taps3(SA_SetGoal, partSize, tap3Positions, 200);
-    ActionSequence taps4(SA_SetGoal, partSize, tap4Positions, 200);
-    ActionSequence taps5(SA_SetGoal, partSize, tap5Positions, 200);
-    ActionSequence taps6(SA_SetGoal, partSize, tap6Positions, 200);
-    ActionSequence taps7(SA_SetGoal, partSize, tap7Positions, 200);
-    ActionSequence taps8(SA_SetGoal, partSize, tap8Positions, 200);
-    ActionSequence tapDown(SA_SetGoal, partSize, waveDownPositions, 80);
-    ActionSequence tapUp(SA_SetGoal, partSize, waveMiddlePositions, 80);
+    ActionSequence* moveStart = new ActionSequence(SA_SetGoal, partSize, homePositions, 1500);
+    ActionSequence* moveUp = new ActionSequence(SA_SetGoal, partSize, upPositions, 1500);
+    ActionSequence* moveDown = new ActionSequence(SA_SetGoal, partSize, downPositions, 1000);
+    ActionSequence* waveUp = new ActionSequence(SA_SetGoal, partSize, waveUpPositions, 1000);
+    ActionSequence* waveDown = new ActionSequence(SA_SetGoal, partSize, waveDownPositions, 1000);
+    ActionSequence* bigwaveUp = new ActionSequence(SA_SetGoal, partSize, bigwaveUpPositions, 1000);
+    ActionSequence* bigwaveDown = new ActionSequence(SA_SetGoal, partSize, bigwaveDownPositions, 1000);
+    ActionSequence* tapsStart = new ActionSequence(SA_SetGoal, partSize, tapStartPositions, 1000);
+    ActionSequence* taps1 = new ActionSequence(SA_SetGoal, partSize, tap1Positions, 300);
+    ActionSequence* taps2 = new ActionSequence(SA_SetGoal, partSize, tap2Positions, 300);
+    ActionSequence* taps3 = new ActionSequence(SA_SetGoal, partSize, tap3Positions, 300);
+    ActionSequence* taps4 = new ActionSequence(SA_SetGoal, partSize, tap4Positions, 300);
+    ActionSequence* taps5 = new ActionSequence(SA_SetGoal, partSize, tap5Positions, 300);
+    ActionSequence* taps6 = new ActionSequence(SA_SetGoal, partSize, tap6Positions, 300);
+    ActionSequence* taps7 = new ActionSequence(SA_SetGoal, partSize, tap7Positions, 300);
+    ActionSequence* taps8 = new ActionSequence(SA_SetGoal, partSize, tap8Positions, 300);
+    ActionSequence* tapDown = new ActionSequence(SA_SetGoal, partSize, waveDownPositions, 500);
+    ActionSequence* tapUp = new ActionSequence(SA_SetGoal, partSize, waveMiddlePositions, 500);
     
-    ActionSequence report(SA_Status);
-    ActionSequence pause2(SA_Delay);
-    pause2.SetParam(2000);
-    ActionSequence loop20(SA_LoopBegin);  
-    loop20.SetParam(20);
-    ActionSequence loop2(SA_LoopBegin);  
-    loop2.SetParam(2);
-    ActionSequence loopEnd(SA_LoopEnd);  
-    ActionSequence pause100ms(SA_Delay);
-    pause100ms.SetParam(100);
-    ActionSequence pause200ms(SA_Delay);
-    pause200ms.SetParam(200);
-    ActionSequence pause500ms(SA_Delay);
-    pause500ms.SetParam(500);
-    ActionSequence pause1(SA_Delay);
-    pause1.SetParam(1000);
+    ActionSequence* pause2 = new ActionSequence(SA_Delay, 0 , NULL, 2000);
+    ActionSequence* loop20 = new ActionSequence(SA_LoopBegin, 0 , NULL, 20);  
+    ActionSequence* loop2 = new ActionSequence(SA_LoopBegin, 0 , NULL, 2);  
+    ActionSequence* loopEnd = new ActionSequence(SA_LoopEnd);
+    ActionSequence* pause100ms = new ActionSequence(SA_Delay, 0 , NULL, 100);
+    ActionSequence* pause200ms = new ActionSequence(SA_Delay, 0 , NULL, 200);
+    ActionSequence* pause500ms = new ActionSequence(SA_Delay, 0 , NULL, 500);
+    ActionSequence* pause1 = new ActionSequence(SA_Delay, 0 , NULL, 1000);
     
     // add actions into StartSeq
     StartSeq.clear();
     StartSeq.push_back(moveUp);
-    StartSeq.push_back(report);
     
     // add actions into WaveSeq
     WaveSeq.clear();
     WaveSeq.push_back(moveStart);
     WaveSeq.push_back(loop2);
     WaveSeq.push_back(waveUp);
-    WaveSeq.push_back(report);
     WaveSeq.push_back(waveDown);
-    WaveSeq.push_back(report);
     WaveSeq.push_back(loopEnd);
     WaveSeq.push_back(moveUp);
     
     // add actions into UpDownSeq
     UpDownSeq.clear();
     UpDownSeq.push_back(moveUp);
-    UpDownSeq.push_back(report);
     UpDownSeq.push_back(pause1);
     UpDownSeq.push_back(moveDown);
-    UpDownSeq.push_back(report);
+    UpDownSeq.push_back(pause1);
     UpDownSeq.push_back(moveUp);
-    UpDownSeq.push_back(pause1);
-    UpDownSeq.push_back(report);
     
     // add actions into RightStepsSeq
     // move to vertical, then move each part one at a time, then return to home
@@ -109,21 +93,20 @@
     
     for (int partIx = 0; partIx < partSize; partIx++)
     {
+        float pos[NUMJOINTS];
         for (int partIy = 0; partIy < partSize; partIy++)
         {
             if (partIx == partIy)
-                rightPositions[partIy] = RightPos;
+                pos[partIy] = RightPos;
             else
-                rightPositions[partIy] = UpPos;
+                pos[partIy] = UpPos;
         }
 
-        ActionSequence* seq = new ActionSequence(SA_SetGoal, partSize, rightPositions, 1000);
-        UpTwistSeq.push_back(*seq);
+        ActionSequence* seq = new ActionSequence(SA_SetGoal, partSize, pos, 1000);
+        UpTwistSeq.push_back(seq);
         UpTwistSeq.push_back(pause1);
-        UpTwistSeq.push_back(report);
     }
     UpTwistSeq.push_back(moveUp);
-    UpTwistSeq.push_back(report);
     
     // add actions to tap sequence
     TapsSeq.clear();
@@ -156,18 +139,16 @@
     TapsSeq.push_back(taps7);
     TapsSeq.push_back(tapDown);
     TapsSeq.push_back(tapUp);
-    TapsSeq.push_back(report);
     TapsSeq.push_back(moveUp);
 
-    // add actions into FastWaveSeq
-    FastWaveSeq.clear();
-    FastWaveSeq.push_back(moveUp);
-    FastWaveSeq.push_back(loop20);
-    FastWaveSeq.push_back(fastwaveUp);
-    FastWaveSeq.push_back(pause100ms);
-    FastWaveSeq.push_back(fastwaveDown);
-    FastWaveSeq.push_back(pause100ms);
-    FastWaveSeq.push_back(loopEnd);
-    FastWaveSeq.push_back(report);
-
+    // add actions into BigWaveSeq
+    BigWaveSeq.clear();
+    BigWaveSeq.push_back(moveUp);
+    BigWaveSeq.push_back(loop20);
+    BigWaveSeq.push_back(bigwaveUp);
+    BigWaveSeq.push_back(pause100ms);
+    BigWaveSeq.push_back(bigwaveDown);
+    BigWaveSeq.push_back(pause100ms);
+    BigWaveSeq.push_back(loopEnd);
+    
 }