File content as of revision 13:ffeff9b5e513:

#include "mbed.h"
#include <vector>
#include <ActionBuf.h>

const float UpPos = 180.0f;
const float RightPos = 90.0f;
const float NoMove = -1.0f;


vector<ActionSequence*> UpDownSeq;
vector<ActionSequence*> UpTwistSeq;
vector<ActionSequence*> StartSeq;
vector<ActionSequence*> WaveSeq;
vector<ActionSequence*> TapsSeq;
vector<ActionSequence*> BigWaveSeq;

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)
{
    // define actions
    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* 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);
    
    // add actions into WaveSeq
    WaveSeq.clear();
    WaveSeq.push_back(moveStart);
    WaveSeq.push_back(loop2);
    WaveSeq.push_back(waveUp);
    WaveSeq.push_back(waveDown);
    WaveSeq.push_back(loopEnd);
    WaveSeq.push_back(moveUp);
    
    // add actions into UpDownSeq
    UpDownSeq.clear();
    UpDownSeq.push_back(moveUp);
    UpDownSeq.push_back(pause1);
    UpDownSeq.push_back(moveDown);
    UpDownSeq.push_back(pause1);
    UpDownSeq.push_back(moveUp);
    
    // 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++)
    {
        float pos[NUMJOINTS];
        for (int partIy = 0; partIy < partSize; partIy++)
        {
            if (partIx == partIy)
                pos[partIy] = RightPos;
            else
                pos[partIy] = UpPos;
        }

        ActionSequence* seq = new ActionSequence(SA_SetGoal, partSize, pos, 1000);
        UpTwistSeq.push_back(seq);
        UpTwistSeq.push_back(pause1);
    }
    UpTwistSeq.push_back(moveUp);
    
    // 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(moveUp);

    // 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);
    
}