File content as of revision 11:3a2e6eb9fbb8:

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

const float UpPos = 180.0f;
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)
{
    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 };
    
    float tapStartPositions[] = { RightPos, 180.0f, 270.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, 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, 400);
    ActionSequence fastwaveDown(SA_SetGoal, partSize, fastwaveDownPositions, 400);
    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 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);
    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(moveUp);
    StartSeq.push_back(report);
    
    // 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(loopEnd);
    WaveSeq.push_back(moveUp);
    
    // add actions into UpDownSeq
    UpDownSeq.clear();
    UpDownSeq.push_back(moveUp);
    UpDownSeq.push_back(report);
    UpDownSeq.push_back(pause1);
    UpDownSeq.push_back(moveDown);
    UpDownSeq.push_back(report);
    UpDownSeq.push_back(moveUp);
    UpDownSeq.push_back(pause1);
    UpDownSeq.push_back(report);
    
    // 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++)
    {
        for (int partIy = 0; partIy < partSize; partIy++)
        {
            if (partIx == partIy)
                rightPositions[partIy] = RightPos;
            else
                rightPositions[partIy] = UpPos;
        }

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

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

}