robot arm demo team / Mbed 2 deprecated RobotArmDemo Featured

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   AX-12A Dynamixel mbed iothub_client EthernetInterface NTPClient ConfigFile SDFileSystem iothub_amqp_transport mbed-rtos proton-c-mbed wolfSSL

Sequences.cpp@13:ffeff9b5e513, 2016-01-15 (annotated)

Committer:
henryrawas
Date:
Fri Jan 15 22:02:46 2016 +0000
Revision:
13:ffeff9b5e513
Parent:
11:3a2e6eb9fbb8
Child:
18:224289104fc0
Always test status and send data

Who changed what in which revision?

UserRevisionLine numberNew contents of line
henryrawas 5:36916b1c5a06 1 #include "mbed.h"
henryrawas 5:36916b1c5a06 2 #include <vector>
henryrawas 5:36916b1c5a06 3 #include <ActionBuf.h>
henryrawas 5:36916b1c5a06 4
henryrawas 5:36916b1c5a06 5 const float UpPos = 180.0f;
henryrawas 7:6723f6887d00 6 const float RightPos = 90.0f;
henryrawas 7:6723f6887d00 7 const float NoMove = -1.0f;
henryrawas 5:36916b1c5a06 8
henryrawas 13:ffeff9b5e513 9
henryrawas 13:ffeff9b5e513 10 vector<ActionSequence*> UpDownSeq;
henryrawas 13:ffeff9b5e513 11 vector<ActionSequence*> UpTwistSeq;
henryrawas 13:ffeff9b5e513 12 vector<ActionSequence*> StartSeq;
henryrawas 13:ffeff9b5e513 13 vector<ActionSequence*> WaveSeq;
henryrawas 13:ffeff9b5e513 14 vector<ActionSequence*> TapsSeq;
henryrawas 13:ffeff9b5e513 15 vector<ActionSequence*> BigWaveSeq;
henryrawas 5:36916b1c5a06 16
henryrawas 13:ffeff9b5e513 17 const float upPositions[] = { UpPos, UpPos, UpPos, UpPos, UpPos };
henryrawas 13:ffeff9b5e513 18 const float downPositions[] = { RightPos, 120.0f, 240.0f, 150.0f, 240.0f };
henryrawas 13:ffeff9b5e513 19 const float homePositions[] = { RightPos, 225.0f, 270.0f, 135.0f, UpPos };
henryrawas 13:ffeff9b5e513 20 const float waveUpPositions[] = { NoMove, NoMove, NoMove, NoMove, 135.0f };
henryrawas 13:ffeff9b5e513 21 const float waveDownPositions[] = { NoMove, NoMove, NoMove, NoMove, 225.0f };
henryrawas 13:ffeff9b5e513 22 const float waveMiddlePositions[] = { NoMove, NoMove, NoMove, NoMove, 180.0f };
henryrawas 13:ffeff9b5e513 23 const float bigwaveUpPositions[] = { NoMove, 135.0f, NoMove, NoMove, NoMove };
henryrawas 13:ffeff9b5e513 24 const float bigwaveDownPositions[] = { NoMove, 225.0f, NoMove, NoMove, NoMove };
henryrawas 13:ffeff9b5e513 25
henryrawas 13:ffeff9b5e513 26 const float tapStartPositions[] = { RightPos, 180.0f, 270.0f, 180.0f, 180.0f };
henryrawas 13:ffeff9b5e513 27 const float tap1Positions[] = { 60.2f, NoMove, NoMove, NoMove, NoMove };
henryrawas 13:ffeff9b5e513 28 const float tap2Positions[] = { 68.7f, NoMove, NoMove, NoMove, NoMove };
henryrawas 13:ffeff9b5e513 29 const float tap3Positions[] = { 77.2f, NoMove, NoMove, NoMove, NoMove };
henryrawas 13:ffeff9b5e513 30 const float tap4Positions[] = { 85.7f, NoMove, NoMove, NoMove, NoMove };
henryrawas 13:ffeff9b5e513 31 const float tap5Positions[] = { 94.2f, NoMove, NoMove, NoMove, NoMove };
henryrawas 13:ffeff9b5e513 32 const float tap6Positions[] = { 102.7f, NoMove, NoMove, NoMove, NoMove };
henryrawas 13:ffeff9b5e513 33 const float tap7Positions[] = { 111.2f, NoMove, NoMove, NoMove, NoMove };
henryrawas 13:ffeff9b5e513 34 const float tap8Positions[] = { 119.7f, NoMove, NoMove, NoMove, NoMove };
henryrawas 13:ffeff9b5e513 35
henryrawas 5:36916b1c5a06 36
henryrawas 13:ffeff9b5e513 37 void MakeSequences(int partSize)
henryrawas 5:36916b1c5a06 38 {
henryrawas 5:36916b1c5a06 39 // define actions
henryrawas 13:ffeff9b5e513 40 ActionSequence* moveStart = new ActionSequence(SA_SetGoal, partSize, homePositions, 1500);
henryrawas 13:ffeff9b5e513 41 ActionSequence* moveUp = new ActionSequence(SA_SetGoal, partSize, upPositions, 1500);
henryrawas 13:ffeff9b5e513 42 ActionSequence* moveDown = new ActionSequence(SA_SetGoal, partSize, downPositions, 1000);
henryrawas 13:ffeff9b5e513 43 ActionSequence* waveUp = new ActionSequence(SA_SetGoal, partSize, waveUpPositions, 1000);
henryrawas 13:ffeff9b5e513 44 ActionSequence* waveDown = new ActionSequence(SA_SetGoal, partSize, waveDownPositions, 1000);
henryrawas 13:ffeff9b5e513 45 ActionSequence* bigwaveUp = new ActionSequence(SA_SetGoal, partSize, bigwaveUpPositions, 1000);
henryrawas 13:ffeff9b5e513 46 ActionSequence* bigwaveDown = new ActionSequence(SA_SetGoal, partSize, bigwaveDownPositions, 1000);
henryrawas 13:ffeff9b5e513 47 ActionSequence* tapsStart = new ActionSequence(SA_SetGoal, partSize, tapStartPositions, 1000);
henryrawas 13:ffeff9b5e513 48 ActionSequence* taps1 = new ActionSequence(SA_SetGoal, partSize, tap1Positions, 300);
henryrawas 13:ffeff9b5e513 49 ActionSequence* taps2 = new ActionSequence(SA_SetGoal, partSize, tap2Positions, 300);
henryrawas 13:ffeff9b5e513 50 ActionSequence* taps3 = new ActionSequence(SA_SetGoal, partSize, tap3Positions, 300);
henryrawas 13:ffeff9b5e513 51 ActionSequence* taps4 = new ActionSequence(SA_SetGoal, partSize, tap4Positions, 300);
henryrawas 13:ffeff9b5e513 52 ActionSequence* taps5 = new ActionSequence(SA_SetGoal, partSize, tap5Positions, 300);
henryrawas 13:ffeff9b5e513 53 ActionSequence* taps6 = new ActionSequence(SA_SetGoal, partSize, tap6Positions, 300);
henryrawas 13:ffeff9b5e513 54 ActionSequence* taps7 = new ActionSequence(SA_SetGoal, partSize, tap7Positions, 300);
henryrawas 13:ffeff9b5e513 55 ActionSequence* taps8 = new ActionSequence(SA_SetGoal, partSize, tap8Positions, 300);
henryrawas 13:ffeff9b5e513 56 ActionSequence* tapDown = new ActionSequence(SA_SetGoal, partSize, waveDownPositions, 500);
henryrawas 13:ffeff9b5e513 57 ActionSequence* tapUp = new ActionSequence(SA_SetGoal, partSize, waveMiddlePositions, 500);
henryrawas 7:6723f6887d00 58
henryrawas 13:ffeff9b5e513 59 ActionSequence* pause2 = new ActionSequence(SA_Delay, 0 , NULL, 2000);
henryrawas 13:ffeff9b5e513 60 ActionSequence* loop20 = new ActionSequence(SA_LoopBegin, 0 , NULL, 20);
henryrawas 13:ffeff9b5e513 61 ActionSequence* loop2 = new ActionSequence(SA_LoopBegin, 0 , NULL, 2);
henryrawas 13:ffeff9b5e513 62 ActionSequence* loopEnd = new ActionSequence(SA_LoopEnd);
henryrawas 13:ffeff9b5e513 63 ActionSequence* pause100ms = new ActionSequence(SA_Delay, 0 , NULL, 100);
henryrawas 13:ffeff9b5e513 64 ActionSequence* pause200ms = new ActionSequence(SA_Delay, 0 , NULL, 200);
henryrawas 13:ffeff9b5e513 65 ActionSequence* pause500ms = new ActionSequence(SA_Delay, 0 , NULL, 500);
henryrawas 13:ffeff9b5e513 66 ActionSequence* pause1 = new ActionSequence(SA_Delay, 0 , NULL, 1000);
henryrawas 8:d98e2dec0f40 67
henryrawas 7:6723f6887d00 68 // add actions into StartSeq
henryrawas 7:6723f6887d00 69 StartSeq.clear();
henryrawas 11:3a2e6eb9fbb8 70 StartSeq.push_back(moveUp);
henryrawas 7:6723f6887d00 71
henryrawas 7:6723f6887d00 72 // add actions into WaveSeq
henryrawas 7:6723f6887d00 73 WaveSeq.clear();
henryrawas 8:d98e2dec0f40 74 WaveSeq.push_back(moveStart);
henryrawas 10:9b21566a5ddb 75 WaveSeq.push_back(loop2);
henryrawas 7:6723f6887d00 76 WaveSeq.push_back(waveUp);
henryrawas 7:6723f6887d00 77 WaveSeq.push_back(waveDown);
henryrawas 10:9b21566a5ddb 78 WaveSeq.push_back(loopEnd);
henryrawas 11:3a2e6eb9fbb8 79 WaveSeq.push_back(moveUp);
henryrawas 7:6723f6887d00 80
henryrawas 5:36916b1c5a06 81 // add actions into UpDownSeq
henryrawas 5:36916b1c5a06 82 UpDownSeq.clear();
henryrawas 5:36916b1c5a06 83 UpDownSeq.push_back(moveUp);
henryrawas 10:9b21566a5ddb 84 UpDownSeq.push_back(pause1);
henryrawas 5:36916b1c5a06 85 UpDownSeq.push_back(moveDown);
henryrawas 13:ffeff9b5e513 86 UpDownSeq.push_back(pause1);
henryrawas 11:3a2e6eb9fbb8 87 UpDownSeq.push_back(moveUp);
henryrawas 5:36916b1c5a06 88
henryrawas 5:36916b1c5a06 89 // add actions into RightStepsSeq
henryrawas 5:36916b1c5a06 90 // move to vertical, then move each part one at a time, then return to home
henryrawas 5:36916b1c5a06 91 UpTwistSeq.clear();
henryrawas 5:36916b1c5a06 92 UpTwistSeq.push_back(moveUp);
henryrawas 5:36916b1c5a06 93
henryrawas 7:6723f6887d00 94 for (int partIx = 0; partIx < partSize; partIx++)
henryrawas 5:36916b1c5a06 95 {
henryrawas 13:ffeff9b5e513 96 float pos[NUMJOINTS];
henryrawas 7:6723f6887d00 97 for (int partIy = 0; partIy < partSize; partIy++)
henryrawas 5:36916b1c5a06 98 {
henryrawas 5:36916b1c5a06 99 if (partIx == partIy)
henryrawas 13:ffeff9b5e513 100 pos[partIy] = RightPos;
henryrawas 10:9b21566a5ddb 101 else
henryrawas 13:ffeff9b5e513 102 pos[partIy] = UpPos;
henryrawas 5:36916b1c5a06 103 }
henryrawas 5:36916b1c5a06 104
henryrawas 13:ffeff9b5e513 105 ActionSequence* seq = new ActionSequence(SA_SetGoal, partSize, pos, 1000);
henryrawas 13:ffeff9b5e513 106 UpTwistSeq.push_back(seq);
henryrawas 10:9b21566a5ddb 107 UpTwistSeq.push_back(pause1);
henryrawas 5:36916b1c5a06 108 }
henryrawas 10:9b21566a5ddb 109 UpTwistSeq.push_back(moveUp);
henryrawas 5:36916b1c5a06 110
henryrawas 8:d98e2dec0f40 111 // add actions to tap sequence
henryrawas 8:d98e2dec0f40 112 TapsSeq.clear();
henryrawas 8:d98e2dec0f40 113 TapsSeq.push_back(tapsStart);
henryrawas 8:d98e2dec0f40 114 TapsSeq.push_back(pause1);
henryrawas 8:d98e2dec0f40 115 TapsSeq.push_back(taps3);
henryrawas 8:d98e2dec0f40 116 TapsSeq.push_back(tapDown);
henryrawas 8:d98e2dec0f40 117 TapsSeq.push_back(tapUp);
henryrawas 8:d98e2dec0f40 118 TapsSeq.push_back(pause100ms);
henryrawas 8:d98e2dec0f40 119 TapsSeq.push_back(taps3);
henryrawas 8:d98e2dec0f40 120 TapsSeq.push_back(tapDown);
henryrawas 8:d98e2dec0f40 121 TapsSeq.push_back(tapUp);
henryrawas 8:d98e2dec0f40 122 TapsSeq.push_back(pause200ms);
henryrawas 8:d98e2dec0f40 123 TapsSeq.push_back(taps1);
henryrawas 8:d98e2dec0f40 124 TapsSeq.push_back(tapDown);
henryrawas 8:d98e2dec0f40 125 TapsSeq.push_back(tapUp);
henryrawas 8:d98e2dec0f40 126 TapsSeq.push_back(pause100ms);
henryrawas 8:d98e2dec0f40 127 TapsSeq.push_back(taps5);
henryrawas 8:d98e2dec0f40 128 TapsSeq.push_back(tapDown);
henryrawas 8:d98e2dec0f40 129 TapsSeq.push_back(tapUp);
henryrawas 8:d98e2dec0f40 130 TapsSeq.push_back(pause100ms);
henryrawas 8:d98e2dec0f40 131 TapsSeq.push_back(taps7);
henryrawas 8:d98e2dec0f40 132 TapsSeq.push_back(tapDown);
henryrawas 8:d98e2dec0f40 133 TapsSeq.push_back(tapUp);
henryrawas 8:d98e2dec0f40 134 TapsSeq.push_back(pause200ms);
henryrawas 8:d98e2dec0f40 135 TapsSeq.push_back(taps6);
henryrawas 8:d98e2dec0f40 136 TapsSeq.push_back(tapDown);
henryrawas 8:d98e2dec0f40 137 TapsSeq.push_back(tapUp);
henryrawas 8:d98e2dec0f40 138 TapsSeq.push_back(pause100ms);
henryrawas 8:d98e2dec0f40 139 TapsSeq.push_back(taps7);
henryrawas 8:d98e2dec0f40 140 TapsSeq.push_back(tapDown);
henryrawas 8:d98e2dec0f40 141 TapsSeq.push_back(tapUp);
henryrawas 11:3a2e6eb9fbb8 142 TapsSeq.push_back(moveUp);
henryrawas 8:d98e2dec0f40 143
henryrawas 13:ffeff9b5e513 144 // add actions into BigWaveSeq
henryrawas 13:ffeff9b5e513 145 BigWaveSeq.clear();
henryrawas 13:ffeff9b5e513 146 BigWaveSeq.push_back(moveUp);
henryrawas 13:ffeff9b5e513 147 BigWaveSeq.push_back(loop20);
henryrawas 13:ffeff9b5e513 148 BigWaveSeq.push_back(bigwaveUp);
henryrawas 13:ffeff9b5e513 149 BigWaveSeq.push_back(pause100ms);
henryrawas 13:ffeff9b5e513 150 BigWaveSeq.push_back(bigwaveDown);
henryrawas 13:ffeff9b5e513 151 BigWaveSeq.push_back(pause100ms);
henryrawas 13:ffeff9b5e513 152 BigWaveSeq.push_back(loopEnd);
henryrawas 13:ffeff9b5e513 153
henryrawas 5:36916b1c5a06 154 }