robot arm demo team / Mbed 2 deprecated RobotArmDemo Featured

Dependencies:   AX-12A Dynamixel mbed iothub_client EthernetInterface NTPClient ConfigFile SDFileSystem iothub_amqp_transport mbed-rtos proton-c-mbed wolfSSL

Revision:
8:d98e2dec0f40
Parent:
7:6723f6887d00
Child:
10:9b21566a5ddb
--- a/Sequences.cpp	Tue Dec 29 23:31:28 2015 +0000
+++ b/Sequences.cpp	Thu Dec 31 17:47:55 2015 +0000
@@ -11,6 +11,7 @@
 vector<ActionSequence> UpTwistSeq;
 vector<ActionSequence> StartSeq;
 vector<ActionSequence> WaveSeq;
+vector<ActionSequence> TapsSeq;
 
 
 void MakeSequences(int partSize, vector<float>& startPositions)
@@ -20,8 +21,19 @@
     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);
@@ -63,22 +75,78 @@
     {
         waveUpPositions.push_back(NoMove);
         waveDownPositions.push_back(NoMove);
+        waveMiddlePositions.push_back(NoMove);
     }
-    waveUpPositions.push_back(120.0f);
-    waveDownPositions.push_back(210.0f);
+    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);
+    }
+    
     // 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 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 report(SA_Status);
     ActionSequence pause2(SA_Delay);
-    pause2.SetDelay(2000);
+    pause2.SetParam(2000);
     ActionSequence rep(SA_Repeat);  
-
+    ActionSequence rep1(SA_Repeat);  
+    rep1.SetParam(1);
+    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);
+    
     // add actions into StartSeq
     StartSeq.clear();
     StartSeq.push_back(moveStart);
@@ -86,11 +154,12 @@
     
     // add actions into WaveSeq
     WaveSeq.clear();
+    WaveSeq.push_back(moveStart);
     WaveSeq.push_back(waveUp);
     WaveSeq.push_back(report);
     WaveSeq.push_back(waveDown);
     WaveSeq.push_back(report);
-    WaveSeq.push_back(rep);
+    WaveSeq.push_back(rep1);
     
     // add actions into UpDownSeq
     UpDownSeq.clear();
@@ -125,6 +194,39 @@
         UpTwistSeq.push_back(moveUp);
     }
     UpTwistSeq.push_back(pause2);
-    UpTwistSeq.push_back(moveDown);
+    UpTwistSeq.push_back(moveStart);
     
+    // add actions to tap sequence
+    TapsSeq.clear();
+    TapsSeq.push_back(tapsStart);
+    TapsSeq.push_back(pause1);
+    TapsSeq.push_back(taps3);
+    TapsSeq.push_back(tapDown);
+    TapsSeq.push_back(tapUp);
+    TapsSeq.push_back(pause100ms);
+    TapsSeq.push_back(taps3);
+    TapsSeq.push_back(tapDown);
+    TapsSeq.push_back(tapUp);
+    TapsSeq.push_back(pause200ms);
+    TapsSeq.push_back(taps1);
+    TapsSeq.push_back(tapDown);
+    TapsSeq.push_back(tapUp);
+    TapsSeq.push_back(pause100ms);
+    TapsSeq.push_back(taps5);
+    TapsSeq.push_back(tapDown);
+    TapsSeq.push_back(tapUp);
+    TapsSeq.push_back(pause100ms);
+    TapsSeq.push_back(taps7);
+    TapsSeq.push_back(tapDown);
+    TapsSeq.push_back(tapUp);
+    TapsSeq.push_back(pause200ms);
+    TapsSeq.push_back(taps6);
+    TapsSeq.push_back(tapDown);
+    TapsSeq.push_back(tapUp);
+    TapsSeq.push_back(pause100ms);
+    TapsSeq.push_back(taps7);
+    TapsSeq.push_back(tapDown);
+    TapsSeq.push_back(tapUp);
+    TapsSeq.push_back(report);
+
 }