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@11:3a2e6eb9fbb8, 2016-01-06 (annotated)

Committer:
henryrawas
Date:
Wed Jan 06 22:25:51 2016 +0000
Revision:
11:3a2e6eb9fbb8
Parent:
10:9b21566a5ddb
Child:
13:ffeff9b5e513
error reset. Better sequences;

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