Sequences.cpp

00001 // Copyright (c) Microsoft. All rights reserved.
00002 // Licensed under the MIT license. See LICENSE file in the project root for full license information.
00003 
00004 #include "mbed.h"
00005 #include <vector>
00006 #include <ActionBuf.h>
00007 
00008 const float UpPos = 180.0f;
00009 const float RightPos = 90.0f;
00010 const float NoMove = -1.0f;
00011 
00012 
00013 vector<const ActionSequence*> UpDownSeq;
00014 vector<const ActionSequence*> UpTwistSeq;
00015 vector<const ActionSequence*> StartSeq;
00016 vector<const ActionSequence*> WaveSeq;
00017 vector<const ActionSequence*> TapsSeq;
00018 vector<const ActionSequence*> BigWaveSeq;
00019 
00020 
00021 void MakeSequences(int partSize)
00022 {
00023     float upPositions[] = { UpPos, UpPos, UpPos, UpPos, UpPos };
00024     float downPositions[] = { RightPos, 120.0f, 240.0f, 150.0f, 240.0f };
00025     float homePositions[] = { RightPos, 225.0f, 270.0f, 135.0f, UpPos };
00026     float waveUpPositions[] = { NoMove, NoMove, NoMove, NoMove, 135.0f };
00027     float waveDownPositions[] = { NoMove, NoMove, NoMove, NoMove, 225.0f };
00028     float waveMiddlePositions[] = { NoMove, NoMove, NoMove, NoMove, 180.0f };
00029     float bigwaveUpPositions[] = { NoMove, 135.0f, NoMove, NoMove, NoMove };
00030     float bigwaveDownPositions[] = { NoMove, 225.0f, NoMove, NoMove, NoMove };
00031     
00032     float tapStartPositions[] = { RightPos, 180.0f, 270.0f, 180.0f, 180.0f };
00033     float tap1Positions[] = { 60.0f, NoMove, NoMove, NoMove, NoMove };
00034     float tap3Positions[] = { 78.0f, NoMove, NoMove, NoMove, NoMove };
00035     float tap7Positions[] = { 112.0f, NoMove, NoMove, NoMove, NoMove };
00036     
00037     // define actions
00038     ActionSequence* moveStart = new ActionSequence(SA_SetGoal, NUMJOINTS, homePositions, 1500);
00039     ActionSequence* moveUp = new ActionSequence(SA_SetGoal, partSize, upPositions, 1500);
00040     ActionSequence* moveDown = new ActionSequence(SA_SetGoal, partSize, downPositions, 1000);
00041     ActionSequence* waveUp = new ActionSequence(SA_SetGoal, partSize, waveUpPositions, 1000);
00042     ActionSequence* waveDown = new ActionSequence(SA_SetGoal, partSize, waveDownPositions, 1000);
00043     ActionSequence* bigwaveUp = new ActionSequence(SA_SetGoal, partSize, bigwaveUpPositions, 1000);
00044     ActionSequence* bigwaveDown = new ActionSequence(SA_SetGoal, partSize, bigwaveDownPositions, 1000);
00045     ActionSequence* tapsStart = new ActionSequence(SA_SetGoal, partSize, tapStartPositions, 1000);
00046     ActionSequence* taps1 = new ActionSequence(SA_SetGoal, partSize, tap1Positions, 300);
00047     ActionSequence* taps3 = new ActionSequence(SA_SetGoal, partSize, tap3Positions, 300);
00048     ActionSequence* taps7 = new ActionSequence(SA_SetGoal, partSize, tap7Positions, 300);
00049     ActionSequence* tapDown = new ActionSequence(SA_SetGoal, partSize, waveDownPositions, 500);
00050     ActionSequence* tapUp = new ActionSequence(SA_SetGoal, partSize, waveMiddlePositions, 500);
00051     
00052     ActionSequence* loop10 = new ActionSequence(SA_LoopBegin, 0 , NULL, 10);  
00053     ActionSequence* loop20 = new ActionSequence(SA_LoopBegin, 0 , NULL, 20);  
00054     ActionSequence* loop1000 = new ActionSequence(SA_LoopBegin, 0 , NULL, 1000);  
00055     ActionSequence* loop2 = new ActionSequence(SA_LoopBegin, 0 , NULL, 2);  
00056     ActionSequence* loopEnd = new ActionSequence(SA_LoopEnd);
00057     ActionSequence* pause100ms = new ActionSequence(SA_Delay, 0 , NULL, 100);
00058     ActionSequence* pause200ms = new ActionSequence(SA_Delay, 0 , NULL, 200);
00059     ActionSequence* pause500ms = new ActionSequence(SA_Delay, 0 , NULL, 500);
00060     ActionSequence* pause1 = new ActionSequence(SA_Delay, 0 , NULL, 1000);
00061     ActionSequence* pause2 = new ActionSequence(SA_Delay, 0 , NULL, 2000);
00062     
00063     // add actions into StartSeq
00064     StartSeq.clear();
00065     StartSeq.push_back(moveUp);
00066     
00067     // add actions into WaveSeq
00068     WaveSeq.clear();
00069     WaveSeq.push_back(moveStart);
00070     WaveSeq.push_back(loop2);
00071     WaveSeq.push_back(waveUp);
00072     WaveSeq.push_back(waveDown);
00073     WaveSeq.push_back(loopEnd);
00074     WaveSeq.push_back(moveUp);
00075     
00076     // add actions into UpDownSeq
00077     UpDownSeq.clear();
00078     UpDownSeq.push_back(loop1000);
00079     UpDownSeq.push_back(moveDown);
00080     UpDownSeq.push_back(pause1);
00081     UpDownSeq.push_back(moveUp);
00082     UpDownSeq.push_back(pause1);
00083     UpDownSeq.push_back(loopEnd);
00084     
00085     // add actions into RightStepsSeq
00086     // move to vertical, then move each part one at a time, then return to home
00087     UpTwistSeq.clear();
00088     UpTwistSeq.push_back(moveUp);
00089     
00090     for (int partIx = 0; partIx < partSize; partIx++)
00091     {
00092         float pos[NUMJOINTS];
00093         for (int partIy = 0; partIy < partSize; partIy++)
00094         {
00095             if (partIx == partIy)
00096                 pos[partIy] = RightPos;
00097             else
00098                 pos[partIy] = UpPos;
00099         }
00100 
00101         ActionSequence* seq = new ActionSequence(SA_SetGoal, partSize, pos, 1000);
00102         UpTwistSeq.push_back(seq);
00103         UpTwistSeq.push_back(pause1);
00104     }
00105     UpTwistSeq.push_back(moveUp);
00106     
00107     // add actions to tap sequence
00108     TapsSeq.clear();
00109     TapsSeq.push_back(tapsStart);
00110     TapsSeq.push_back(loop10);
00111     TapsSeq.push_back(pause1);
00112     TapsSeq.push_back(taps1);
00113     TapsSeq.push_back(tapDown);
00114     TapsSeq.push_back(tapUp);
00115     TapsSeq.push_back(pause100ms);
00116     TapsSeq.push_back(taps3);
00117     TapsSeq.push_back(tapDown);
00118     TapsSeq.push_back(tapUp);
00119     TapsSeq.push_back(pause200ms);
00120     TapsSeq.push_back(taps7);
00121     TapsSeq.push_back(tapDown);
00122     TapsSeq.push_back(tapUp);
00123     TapsSeq.push_back(loopEnd);
00124     TapsSeq.push_back(moveUp);
00125 
00126     // add actions into BigWaveSeq
00127     BigWaveSeq.clear();
00128     BigWaveSeq.push_back(moveUp);
00129     BigWaveSeq.push_back(loop20);
00130     BigWaveSeq.push_back(bigwaveUp);
00131     BigWaveSeq.push_back(pause100ms);
00132     BigWaveSeq.push_back(bigwaveDown);
00133     BigWaveSeq.push_back(pause100ms);
00134     BigWaveSeq.push_back(loopEnd);
00135     
00136 }