File content as of revision 7:6723f6887d00:

#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;


void MakeSequences(int partSize, vector<float>& startPositions)
{
    vector<float> upPositions;
    vector<float> downPositions;
    vector<float> homePositions;
    vector<float> waveUpPositions;
    vector<float> waveDownPositions;
    vector<float> rightPositions;
    
    for (int partIx = 0; partIx < partSize; partIx++)
    {
        upPositions.push_back(UpPos);
    }
    
    for (int partIx = 0; partIx < partSize; partIx++)
    {
        rightPositions.push_back(UpPos);
    }
    
    // define normal start position
    homePositions.push_back(RightPos);
    if (partSize > 3)
    {
        homePositions.push_back(225.0f);
        homePositions.push_back(270.0f);
        homePositions.push_back(135.0f);
        for (int partIx = 4; partIx < partSize; partIx++)
        {
            homePositions.push_back(180.0f);
        }
    }

    // define down position
    downPositions.push_back(RightPos);
    if (partSize > 3)
    {
        downPositions.push_back(120.0f);
        downPositions.push_back(240.0f);
        downPositions.push_back(150.0f);
        for (int partIx = 4; partIx < partSize; partIx++)
        {
            downPositions.push_back(240.0f);
        }
    }
    
    // define wave up and wave down to only move last part
    for (int partIx = 0; partIx < partSize - 1; partIx++)
    {
        waveUpPositions.push_back(NoMove);
        waveDownPositions.push_back(NoMove);
    }
    waveUpPositions.push_back(120.0f);
    waveDownPositions.push_back(210.0f);

    // 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 report(SA_Status);
    ActionSequence pause2(SA_Delay);
    pause2.SetDelay(2000);
    ActionSequence rep(SA_Repeat);  

    // add actions into StartSeq
    StartSeq.clear();
    StartSeq.push_back(moveStart);
    StartSeq.push_back(report);
    
    // add actions into WaveSeq
    WaveSeq.clear();
    WaveSeq.push_back(waveUp);
    WaveSeq.push_back(report);
    WaveSeq.push_back(waveDown);
    WaveSeq.push_back(report);
    WaveSeq.push_back(rep);
    
    // add actions into UpDownSeq
    UpDownSeq.clear();
    UpDownSeq.push_back(moveUp);
    UpDownSeq.push_back(report);
    UpDownSeq.push_back(pause2);
    UpDownSeq.push_back(moveDown);
    UpDownSeq.push_back(report);
    UpDownSeq.push_back(pause2);
    UpDownSeq.push_back(rep);
    
    // 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++)
    {
        vector<float>* rightPositions = new vector<float>();
        for (int partIy = 0; partIy < partSize; partIy++)
        {
            float val = UpPos;
            if (partIx == partIy)
                val = RightPos;
            rightPositions->push_back(val);
        }

        ActionSequence* seq = new ActionSequence(SA_SetGoal, *rightPositions, 1000);
        UpTwistSeq.push_back(*seq);
        UpTwistSeq.push_back(pause2);
        UpTwistSeq.push_back(report);
        UpTwistSeq.push_back(moveUp);
    }
    UpTwistSeq.push_back(pause2);
    UpTwistSeq.push_back(moveDown);
    
}