File content as of revision 8:d98e2dec0f40:

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


void MakeSequences(int partSize, vector<float>& startPositions)
{
    vector<float> upPositions;
    vector<float> downPositions;
    vector<float> homePositions;
    vector<float> waveUpPositions;
    vector<float> waveDownPositions;
    vector<float> waveMiddlePositions;
    vector<float> rightPositions;
    
    vector<float> tapStartPositions;
    vector<float> tap1Positions;
    vector<float> tap2Positions;
    vector<float> tap3Positions;
    vector<float> tap4Positions;
    vector<float> tap5Positions;
    vector<float> tap6Positions;
    vector<float> tap7Positions;
    vector<float> tap8Positions;
    
    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);
        waveMiddlePositions.push_back(NoMove);
    }
    waveUpPositions.push_back(135.0f);
    waveDownPositions.push_back(225.0f);
    waveMiddlePositions.push_back(180.0f);

    // define tapping start
    tapStartPositions.push_back(RightPos);
    if (partSize > 3)
    {
        tapStartPositions.push_back(135.0f);
        tapStartPositions.push_back(225.0f);
        tapStartPositions.push_back(180.0f);
        for (int partIx = 4; partIx < partSize; partIx++)
        {
            tapStartPositions.push_back(180.0f);
        }
    }
    // define other taps to just swivel - no other moves
    tap1Positions.push_back(60.2f);
    tap2Positions.push_back(68.7f);
    tap3Positions.push_back(77.2f);
    tap4Positions.push_back(85.7f);
    tap5Positions.push_back(94.2f);
    tap6Positions.push_back(102.7f);
    tap7Positions.push_back(111.2f);
    tap8Positions.push_back(119.7f);
    for (int partIx = 1; partIx < partSize; partIx++)
    {
        tap1Positions.push_back(NoMove);
        tap2Positions.push_back(NoMove);
        tap3Positions.push_back(NoMove);
        tap4Positions.push_back(NoMove);
        tap5Positions.push_back(NoMove);
        tap6Positions.push_back(NoMove);
        tap7Positions.push_back(NoMove);
        tap8Positions.push_back(NoMove);
    }
    
    // define actions
    ActionSequence moveStart(SA_SetGoal, homePositions, 1500);
    ActionSequence moveUp(SA_SetGoal, upPositions, 1000);
    ActionSequence moveDown(SA_SetGoal, downPositions, 1000);
    ActionSequence waveUp(SA_SetGoal, waveUpPositions, 1000);
    ActionSequence waveDown(SA_SetGoal, waveDownPositions, 1000);
    ActionSequence tapsStart(SA_SetGoal, tapStartPositions, 1000);
    ActionSequence taps1(SA_SetGoal, tap1Positions, 200);
    ActionSequence taps2(SA_SetGoal, tap2Positions, 200);
    ActionSequence taps3(SA_SetGoal, tap3Positions, 200);
    ActionSequence taps4(SA_SetGoal, tap4Positions, 200);
    ActionSequence taps5(SA_SetGoal, tap5Positions, 200);
    ActionSequence taps6(SA_SetGoal, tap6Positions, 200);
    ActionSequence taps7(SA_SetGoal, tap7Positions, 200);
    ActionSequence taps8(SA_SetGoal, tap8Positions, 200);
    ActionSequence tapDown(SA_SetGoal, waveDownPositions, 80);
    ActionSequence tapUp(SA_SetGoal, waveMiddlePositions, 80);
    
    ActionSequence report(SA_Status);
    ActionSequence pause2(SA_Delay);
    pause2.SetParam(2000);
    ActionSequence rep(SA_Repeat);  
    ActionSequence rep1(SA_Repeat);  
    rep1.SetParam(1);
    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(moveStart);
    StartSeq.push_back(report);
    
    // add actions into WaveSeq
    WaveSeq.clear();
    WaveSeq.push_back(moveStart);
    WaveSeq.push_back(waveUp);
    WaveSeq.push_back(report);
    WaveSeq.push_back(waveDown);
    WaveSeq.push_back(report);
    WaveSeq.push_back(rep1);
    
    // 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(moveStart);
    
    // 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);

}