Diff: Sequences.cpp

Revision:

10:9b21566a5ddb

Parent:

8:d98e2dec0f40

Child:

11:3a2e6eb9fbb8

--- a/Sequences.cpp	Thu Dec 31 20:02:58 2015 +0000
+++ b/Sequences.cpp	Wed Jan 06 00:58:41 2016 +0000
@@ -6,138 +6,66 @@
 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> FastWaveSeq;
 
 void MakeSequences(int partSize, vector<float>& startPositions)
 {
-    vector<float> upPositions;
-    vector<float> downPositions;
-    vector<float> homePositions;
-    vector<float> waveUpPositions;
-    vector<float> waveDownPositions;
-    vector<float> waveMiddlePositions;
-    vector<float> rightPositions;
-    
-    vector<float> tapStartPositions;
-    vector<float> tap1Positions;
-    vector<float> tap2Positions;
-    vector<float> tap3Positions;
-    vector<float> tap4Positions;
-    vector<float> tap5Positions;
-    vector<float> tap6Positions;
-    vector<float> tap7Positions;
-    vector<float> tap8Positions;
-    
-    for (int partIx = 0; partIx < partSize; partIx++)
-    {
-        upPositions.push_back(UpPos);
-    }
-    
-    for (int partIx = 0; partIx < partSize; partIx++)
-    {
-        rightPositions.push_back(UpPos);
-    }
+    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 };
     
-    // 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);
-        waveMiddlePositions.push_back(NoMove);
-    }
-    waveUpPositions.push_back(135.0f);
-    waveDownPositions.push_back(225.0f);
-    waveMiddlePositions.push_back(180.0f);
-
-    // define tapping start
-    tapStartPositions.push_back(RightPos);
-    if (partSize > 3)
-    {
-        tapStartPositions.push_back(135.0f);
-        tapStartPositions.push_back(225.0f);
-        tapStartPositions.push_back(180.0f);
-        for (int partIx = 4; partIx < partSize; partIx++)
-        {
-            tapStartPositions.push_back(180.0f);
-        }
-    }
-    // define other taps to just swivel - no other moves
-    tap1Positions.push_back(60.2f);
-    tap2Positions.push_back(68.7f);
-    tap3Positions.push_back(77.2f);
-    tap4Positions.push_back(85.7f);
-    tap5Positions.push_back(94.2f);
-    tap6Positions.push_back(102.7f);
-    tap7Positions.push_back(111.2f);
-    tap8Positions.push_back(119.7f);
-    for (int partIx = 1; partIx < partSize; partIx++)
-    {
-        tap1Positions.push_back(NoMove);
-        tap2Positions.push_back(NoMove);
-        tap3Positions.push_back(NoMove);
-        tap4Positions.push_back(NoMove);
-        tap5Positions.push_back(NoMove);
-        tap6Positions.push_back(NoMove);
-        tap7Positions.push_back(NoMove);
-        tap8Positions.push_back(NoMove);
-    }
+    float tapStartPositions[] = { RightPos, 135.0f, 225.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, homePositions, 1500);
-    ActionSequence moveUp(SA_SetGoal, upPositions, 1000);
-    ActionSequence moveDown(SA_SetGoal, downPositions, 1000);
-    ActionSequence waveUp(SA_SetGoal, waveUpPositions, 1000);
-    ActionSequence waveDown(SA_SetGoal, waveDownPositions, 1000);
-    ActionSequence tapsStart(SA_SetGoal, tapStartPositions, 1000);
-    ActionSequence taps1(SA_SetGoal, tap1Positions, 200);
-    ActionSequence taps2(SA_SetGoal, tap2Positions, 200);
-    ActionSequence taps3(SA_SetGoal, tap3Positions, 200);
-    ActionSequence taps4(SA_SetGoal, tap4Positions, 200);
-    ActionSequence taps5(SA_SetGoal, tap5Positions, 200);
-    ActionSequence taps6(SA_SetGoal, tap6Positions, 200);
-    ActionSequence taps7(SA_SetGoal, tap7Positions, 200);
-    ActionSequence taps8(SA_SetGoal, tap8Positions, 200);
-    ActionSequence tapDown(SA_SetGoal, waveDownPositions, 80);
-    ActionSequence tapUp(SA_SetGoal, waveMiddlePositions, 80);
+    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, 200);
+    ActionSequence fastwaveDown(SA_SetGoal, partSize, fastwaveDownPositions, 200);
+    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 report(SA_Status);
     ActionSequence pause2(SA_Delay);
     pause2.SetParam(2000);
-    ActionSequence rep(SA_Repeat);  
-    ActionSequence rep1(SA_Repeat);  
-    rep1.SetParam(1);
+    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);
@@ -155,21 +83,20 @@
     // 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(rep1);
+    WaveSeq.push_back(loopEnd);
     
     // add actions into UpDownSeq
     UpDownSeq.clear();
     UpDownSeq.push_back(moveUp);
     UpDownSeq.push_back(report);
-    UpDownSeq.push_back(pause2);
+    UpDownSeq.push_back(pause1);
     UpDownSeq.push_back(moveDown);
     UpDownSeq.push_back(report);
-    UpDownSeq.push_back(pause2);
-    UpDownSeq.push_back(rep);
     
     // add actions into RightStepsSeq
     // move to vertical, then move each part one at a time, then return to home
@@ -178,23 +105,21 @@
     
     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);
+                rightPositions[partIy] = RightPos;
+            else
+                rightPositions[partIy] = UpPos;
         }
 
-        ActionSequence* seq = new ActionSequence(SA_SetGoal, *rightPositions, 1000);
+        ActionSequence* seq = new ActionSequence(SA_SetGoal, partSize, rightPositions, 1000);
         UpTwistSeq.push_back(*seq);
-        UpTwistSeq.push_back(pause2);
+        UpTwistSeq.push_back(pause1);
         UpTwistSeq.push_back(report);
-        UpTwistSeq.push_back(moveUp);
     }
-    UpTwistSeq.push_back(pause2);
-    UpTwistSeq.push_back(moveStart);
+    UpTwistSeq.push_back(moveUp);
+    UpTwistSeq.push_back(report);
     
     // add actions to tap sequence
     TapsSeq.clear();
@@ -229,4 +154,15 @@
     TapsSeq.push_back(tapUp);
     TapsSeq.push_back(report);
 
+    // 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);
+
 }