File content as of revision 33:8b9dcbf6d8ec:

// Copyright (c) Microsoft. All rights reserved.
// Licensed under the MIT license. See LICENSE file in the project root for full license information.

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

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


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


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 waveUpPositions[] = { NoMove, NoMove, NoMove, NoMove, 135.0f };
    float waveDownPositions[] = { NoMove, NoMove, NoMove, NoMove, 225.0f };
    float waveMiddlePositions[] = { NoMove, NoMove, NoMove, NoMove, 180.0f };
    float bigwaveUpPositions[] = { NoMove, 135.0f, NoMove, NoMove, NoMove };
    float bigwaveDownPositions[] = { NoMove, 225.0f, NoMove, NoMove, NoMove };
    
    float tapStartPositions[] = { RightPos, 180.0f, 270.0f, 180.0f, 180.0f };
    float tap1Positions[] = { 60.0f, NoMove, NoMove, NoMove, NoMove };
    float tap3Positions[] = { 78.0f, NoMove, NoMove, NoMove, NoMove };
    float tap7Positions[] = { 112.0f, NoMove, NoMove, NoMove, NoMove };
    
    // define actions
    ActionSequence* moveStart = new ActionSequence(SA_SetGoal, NUMJOINTS, 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* taps3 = new ActionSequence(SA_SetGoal, partSize, tap3Positions, 300);
    ActionSequence* taps7 = new ActionSequence(SA_SetGoal, partSize, tap7Positions, 300);
    ActionSequence* tapDown = new ActionSequence(SA_SetGoal, partSize, waveDownPositions, 500);
    ActionSequence* tapUp = new ActionSequence(SA_SetGoal, partSize, waveMiddlePositions, 500);
    
    ActionSequence* loop10 = new ActionSequence(SA_LoopBegin, 0 , NULL, 10);  
    ActionSequence* loop20 = new ActionSequence(SA_LoopBegin, 0 , NULL, 20);  
    ActionSequence* loop1000 = new ActionSequence(SA_LoopBegin, 0 , NULL, 1000);  
    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);
    ActionSequence* pause2 = new ActionSequence(SA_Delay, 0 , NULL, 2000);
    
    // 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(loop1000);
    UpDownSeq.push_back(moveDown);
    UpDownSeq.push_back(pause1);
    UpDownSeq.push_back(moveUp);
    UpDownSeq.push_back(pause1);
    UpDownSeq.push_back(loopEnd);
    
    // 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(loop10);
    TapsSeq.push_back(pause1);
    TapsSeq.push_back(taps1);
    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(taps7);
    TapsSeq.push_back(tapDown);
    TapsSeq.push_back(tapUp);
    TapsSeq.push_back(loopEnd);
    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);
    
}