robot arm demo team / Mbed 2 deprecated RobotArmDemo Featured

demo project

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

Sequences.cpp@8:d98e2dec0f40, 2015-12-31 (annotated)

Committer:
henryrawas
Date:
Thu Dec 31 17:47:55 2015 +0000
Revision:
8:d98e2dec0f40
Parent:
7:6723f6887d00
Child:
10:9b21566a5ddb
add taps

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 5:36916b1c5a06 9
henryrawas 5:36916b1c5a06 10 vector<ActionSequence> UpDownSeq;
henryrawas 5:36916b1c5a06 11 vector<ActionSequence> UpTwistSeq;
henryrawas 7:6723f6887d00 12 vector<ActionSequence> StartSeq;
henryrawas 7:6723f6887d00 13 vector<ActionSequence> WaveSeq;
henryrawas 8:d98e2dec0f40 14 vector<ActionSequence> TapsSeq;
henryrawas 5:36916b1c5a06 15
henryrawas 5:36916b1c5a06 16
henryrawas 7:6723f6887d00 17 void MakeSequences(int partSize, vector<float>& startPositions)
henryrawas 5:36916b1c5a06 18 {
henryrawas 5:36916b1c5a06 19 vector<float> upPositions;
henryrawas 7:6723f6887d00 20 vector<float> downPositions;
henryrawas 7:6723f6887d00 21 vector<float> homePositions;
henryrawas 7:6723f6887d00 22 vector<float> waveUpPositions;
henryrawas 7:6723f6887d00 23 vector<float> waveDownPositions;
henryrawas 8:d98e2dec0f40 24 vector<float> waveMiddlePositions;
henryrawas 7:6723f6887d00 25 vector<float> rightPositions;
henryrawas 7:6723f6887d00 26
henryrawas 8:d98e2dec0f40 27 vector<float> tapStartPositions;
henryrawas 8:d98e2dec0f40 28 vector<float> tap1Positions;
henryrawas 8:d98e2dec0f40 29 vector<float> tap2Positions;
henryrawas 8:d98e2dec0f40 30 vector<float> tap3Positions;
henryrawas 8:d98e2dec0f40 31 vector<float> tap4Positions;
henryrawas 8:d98e2dec0f40 32 vector<float> tap5Positions;
henryrawas 8:d98e2dec0f40 33 vector<float> tap6Positions;
henryrawas 8:d98e2dec0f40 34 vector<float> tap7Positions;
henryrawas 8:d98e2dec0f40 35 vector<float> tap8Positions;
henryrawas 8:d98e2dec0f40 36
henryrawas 7:6723f6887d00 37 for (int partIx = 0; partIx < partSize; partIx++)
henryrawas 5:36916b1c5a06 38 {
henryrawas 5:36916b1c5a06 39 upPositions.push_back(UpPos);
henryrawas 5:36916b1c5a06 40 }
henryrawas 7:6723f6887d00 41
henryrawas 7:6723f6887d00 42 for (int partIx = 0; partIx < partSize; partIx++)
henryrawas 7:6723f6887d00 43 {
henryrawas 7:6723f6887d00 44 rightPositions.push_back(UpPos);
henryrawas 7:6723f6887d00 45 }
henryrawas 7:6723f6887d00 46
henryrawas 7:6723f6887d00 47 // define normal start position
henryrawas 7:6723f6887d00 48 homePositions.push_back(RightPos);
henryrawas 7:6723f6887d00 49 if (partSize > 3)
henryrawas 7:6723f6887d00 50 {
henryrawas 7:6723f6887d00 51 homePositions.push_back(225.0f);
henryrawas 7:6723f6887d00 52 homePositions.push_back(270.0f);
henryrawas 7:6723f6887d00 53 homePositions.push_back(135.0f);
henryrawas 7:6723f6887d00 54 for (int partIx = 4; partIx < partSize; partIx++)
henryrawas 7:6723f6887d00 55 {
henryrawas 7:6723f6887d00 56 homePositions.push_back(180.0f);
henryrawas 7:6723f6887d00 57 }
henryrawas 7:6723f6887d00 58 }
henryrawas 7:6723f6887d00 59
henryrawas 7:6723f6887d00 60 // define down position
henryrawas 7:6723f6887d00 61 downPositions.push_back(RightPos);
henryrawas 7:6723f6887d00 62 if (partSize > 3)
henryrawas 7:6723f6887d00 63 {
henryrawas 7:6723f6887d00 64 downPositions.push_back(120.0f);
henryrawas 7:6723f6887d00 65 downPositions.push_back(240.0f);
henryrawas 7:6723f6887d00 66 downPositions.push_back(150.0f);
henryrawas 7:6723f6887d00 67 for (int partIx = 4; partIx < partSize; partIx++)
henryrawas 7:6723f6887d00 68 {
henryrawas 7:6723f6887d00 69 downPositions.push_back(240.0f);
henryrawas 7:6723f6887d00 70 }
henryrawas 7:6723f6887d00 71 }
henryrawas 7:6723f6887d00 72
henryrawas 7:6723f6887d00 73 // define wave up and wave down to only move last part
henryrawas 7:6723f6887d00 74 for (int partIx = 0; partIx < partSize - 1; partIx++)
henryrawas 7:6723f6887d00 75 {
henryrawas 7:6723f6887d00 76 waveUpPositions.push_back(NoMove);
henryrawas 7:6723f6887d00 77 waveDownPositions.push_back(NoMove);
henryrawas 8:d98e2dec0f40 78 waveMiddlePositions.push_back(NoMove);
henryrawas 7:6723f6887d00 79 }
henryrawas 8:d98e2dec0f40 80 waveUpPositions.push_back(135.0f);
henryrawas 8:d98e2dec0f40 81 waveDownPositions.push_back(225.0f);
henryrawas 8:d98e2dec0f40 82 waveMiddlePositions.push_back(180.0f);
henryrawas 5:36916b1c5a06 83
henryrawas 8:d98e2dec0f40 84 // define tapping start
henryrawas 8:d98e2dec0f40 85 tapStartPositions.push_back(RightPos);
henryrawas 8:d98e2dec0f40 86 if (partSize > 3)
henryrawas 8:d98e2dec0f40 87 {
henryrawas 8:d98e2dec0f40 88 tapStartPositions.push_back(135.0f);
henryrawas 8:d98e2dec0f40 89 tapStartPositions.push_back(225.0f);
henryrawas 8:d98e2dec0f40 90 tapStartPositions.push_back(180.0f);
henryrawas 8:d98e2dec0f40 91 for (int partIx = 4; partIx < partSize; partIx++)
henryrawas 8:d98e2dec0f40 92 {
henryrawas 8:d98e2dec0f40 93 tapStartPositions.push_back(180.0f);
henryrawas 8:d98e2dec0f40 94 }
henryrawas 8:d98e2dec0f40 95 }
henryrawas 8:d98e2dec0f40 96 // define other taps to just swivel - no other moves
henryrawas 8:d98e2dec0f40 97 tap1Positions.push_back(60.2f);
henryrawas 8:d98e2dec0f40 98 tap2Positions.push_back(68.7f);
henryrawas 8:d98e2dec0f40 99 tap3Positions.push_back(77.2f);
henryrawas 8:d98e2dec0f40 100 tap4Positions.push_back(85.7f);
henryrawas 8:d98e2dec0f40 101 tap5Positions.push_back(94.2f);
henryrawas 8:d98e2dec0f40 102 tap6Positions.push_back(102.7f);
henryrawas 8:d98e2dec0f40 103 tap7Positions.push_back(111.2f);
henryrawas 8:d98e2dec0f40 104 tap8Positions.push_back(119.7f);
henryrawas 8:d98e2dec0f40 105 for (int partIx = 1; partIx < partSize; partIx++)
henryrawas 8:d98e2dec0f40 106 {
henryrawas 8:d98e2dec0f40 107 tap1Positions.push_back(NoMove);
henryrawas 8:d98e2dec0f40 108 tap2Positions.push_back(NoMove);
henryrawas 8:d98e2dec0f40 109 tap3Positions.push_back(NoMove);
henryrawas 8:d98e2dec0f40 110 tap4Positions.push_back(NoMove);
henryrawas 8:d98e2dec0f40 111 tap5Positions.push_back(NoMove);
henryrawas 8:d98e2dec0f40 112 tap6Positions.push_back(NoMove);
henryrawas 8:d98e2dec0f40 113 tap7Positions.push_back(NoMove);
henryrawas 8:d98e2dec0f40 114 tap8Positions.push_back(NoMove);
henryrawas 8:d98e2dec0f40 115 }
henryrawas 8:d98e2dec0f40 116
henryrawas 5:36916b1c5a06 117 // define actions
henryrawas 7:6723f6887d00 118 ActionSequence moveStart(SA_SetGoal, homePositions, 1500);
henryrawas 8:d98e2dec0f40 119 ActionSequence moveUp(SA_SetGoal, upPositions, 1000);
henryrawas 8:d98e2dec0f40 120 ActionSequence moveDown(SA_SetGoal, downPositions, 1000);
henryrawas 8:d98e2dec0f40 121 ActionSequence waveUp(SA_SetGoal, waveUpPositions, 1000);
henryrawas 8:d98e2dec0f40 122 ActionSequence waveDown(SA_SetGoal, waveDownPositions, 1000);
henryrawas 8:d98e2dec0f40 123 ActionSequence tapsStart(SA_SetGoal, tapStartPositions, 1000);
henryrawas 8:d98e2dec0f40 124 ActionSequence taps1(SA_SetGoal, tap1Positions, 200);
henryrawas 8:d98e2dec0f40 125 ActionSequence taps2(SA_SetGoal, tap2Positions, 200);
henryrawas 8:d98e2dec0f40 126 ActionSequence taps3(SA_SetGoal, tap3Positions, 200);
henryrawas 8:d98e2dec0f40 127 ActionSequence taps4(SA_SetGoal, tap4Positions, 200);
henryrawas 8:d98e2dec0f40 128 ActionSequence taps5(SA_SetGoal, tap5Positions, 200);
henryrawas 8:d98e2dec0f40 129 ActionSequence taps6(SA_SetGoal, tap6Positions, 200);
henryrawas 8:d98e2dec0f40 130 ActionSequence taps7(SA_SetGoal, tap7Positions, 200);
henryrawas 8:d98e2dec0f40 131 ActionSequence taps8(SA_SetGoal, tap8Positions, 200);
henryrawas 8:d98e2dec0f40 132 ActionSequence tapDown(SA_SetGoal, waveDownPositions, 80);
henryrawas 8:d98e2dec0f40 133 ActionSequence tapUp(SA_SetGoal, waveMiddlePositions, 80);
henryrawas 7:6723f6887d00 134
henryrawas 5:36916b1c5a06 135 ActionSequence report(SA_Status);
henryrawas 5:36916b1c5a06 136 ActionSequence pause2(SA_Delay);
henryrawas 8:d98e2dec0f40 137 pause2.SetParam(2000);
henryrawas 5:36916b1c5a06 138 ActionSequence rep(SA_Repeat);
henryrawas 8:d98e2dec0f40 139 ActionSequence rep1(SA_Repeat);
henryrawas 8:d98e2dec0f40 140 rep1.SetParam(1);
henryrawas 8:d98e2dec0f40 141 ActionSequence pause100ms(SA_Delay);
henryrawas 8:d98e2dec0f40 142 pause100ms.SetParam(100);
henryrawas 8:d98e2dec0f40 143 ActionSequence pause200ms(SA_Delay);
henryrawas 8:d98e2dec0f40 144 pause200ms.SetParam(200);
henryrawas 8:d98e2dec0f40 145 ActionSequence pause500ms(SA_Delay);
henryrawas 8:d98e2dec0f40 146 pause500ms.SetParam(500);
henryrawas 8:d98e2dec0f40 147 ActionSequence pause1(SA_Delay);
henryrawas 8:d98e2dec0f40 148 pause1.SetParam(1000);
henryrawas 8:d98e2dec0f40 149
henryrawas 7:6723f6887d00 150 // add actions into StartSeq
henryrawas 7:6723f6887d00 151 StartSeq.clear();
henryrawas 7:6723f6887d00 152 StartSeq.push_back(moveStart);
henryrawas 7:6723f6887d00 153 StartSeq.push_back(report);
henryrawas 7:6723f6887d00 154
henryrawas 7:6723f6887d00 155 // add actions into WaveSeq
henryrawas 7:6723f6887d00 156 WaveSeq.clear();
henryrawas 8:d98e2dec0f40 157 WaveSeq.push_back(moveStart);
henryrawas 7:6723f6887d00 158 WaveSeq.push_back(waveUp);
henryrawas 7:6723f6887d00 159 WaveSeq.push_back(report);
henryrawas 7:6723f6887d00 160 WaveSeq.push_back(waveDown);
henryrawas 7:6723f6887d00 161 WaveSeq.push_back(report);
henryrawas 8:d98e2dec0f40 162 WaveSeq.push_back(rep1);
henryrawas 7:6723f6887d00 163
henryrawas 5:36916b1c5a06 164 // add actions into UpDownSeq
henryrawas 5:36916b1c5a06 165 UpDownSeq.clear();
henryrawas 5:36916b1c5a06 166 UpDownSeq.push_back(moveUp);
henryrawas 5:36916b1c5a06 167 UpDownSeq.push_back(report);
henryrawas 5:36916b1c5a06 168 UpDownSeq.push_back(pause2);
henryrawas 5:36916b1c5a06 169 UpDownSeq.push_back(moveDown);
henryrawas 5:36916b1c5a06 170 UpDownSeq.push_back(report);
henryrawas 5:36916b1c5a06 171 UpDownSeq.push_back(pause2);
henryrawas 5:36916b1c5a06 172 UpDownSeq.push_back(rep);
henryrawas 5:36916b1c5a06 173
henryrawas 5:36916b1c5a06 174 // add actions into RightStepsSeq
henryrawas 5:36916b1c5a06 175 // move to vertical, then move each part one at a time, then return to home
henryrawas 5:36916b1c5a06 176 UpTwistSeq.clear();
henryrawas 5:36916b1c5a06 177 UpTwistSeq.push_back(moveUp);
henryrawas 5:36916b1c5a06 178
henryrawas 7:6723f6887d00 179 for (int partIx = 0; partIx < partSize; partIx++)
henryrawas 5:36916b1c5a06 180 {
henryrawas 5:36916b1c5a06 181 vector<float>* rightPositions = new vector<float>();
henryrawas 7:6723f6887d00 182 for (int partIy = 0; partIy < partSize; partIy++)
henryrawas 5:36916b1c5a06 183 {
henryrawas 5:36916b1c5a06 184 float val = UpPos;
henryrawas 5:36916b1c5a06 185 if (partIx == partIy)
henryrawas 7:6723f6887d00 186 val = RightPos;
henryrawas 5:36916b1c5a06 187 rightPositions->push_back(val);
henryrawas 5:36916b1c5a06 188 }
henryrawas 5:36916b1c5a06 189
henryrawas 5:36916b1c5a06 190 ActionSequence* seq = new ActionSequence(SA_SetGoal, *rightPositions, 1000);
henryrawas 5:36916b1c5a06 191 UpTwistSeq.push_back(*seq);
henryrawas 5:36916b1c5a06 192 UpTwistSeq.push_back(pause2);
henryrawas 5:36916b1c5a06 193 UpTwistSeq.push_back(report);
henryrawas 5:36916b1c5a06 194 UpTwistSeq.push_back(moveUp);
henryrawas 5:36916b1c5a06 195 }
henryrawas 5:36916b1c5a06 196 UpTwistSeq.push_back(pause2);
henryrawas 8:d98e2dec0f40 197 UpTwistSeq.push_back(moveStart);
henryrawas 5:36916b1c5a06 198
henryrawas 8:d98e2dec0f40 199 // add actions to tap sequence
henryrawas 8:d98e2dec0f40 200 TapsSeq.clear();
henryrawas 8:d98e2dec0f40 201 TapsSeq.push_back(tapsStart);
henryrawas 8:d98e2dec0f40 202 TapsSeq.push_back(pause1);
henryrawas 8:d98e2dec0f40 203 TapsSeq.push_back(taps3);
henryrawas 8:d98e2dec0f40 204 TapsSeq.push_back(tapDown);
henryrawas 8:d98e2dec0f40 205 TapsSeq.push_back(tapUp);
henryrawas 8:d98e2dec0f40 206 TapsSeq.push_back(pause100ms);
henryrawas 8:d98e2dec0f40 207 TapsSeq.push_back(taps3);
henryrawas 8:d98e2dec0f40 208 TapsSeq.push_back(tapDown);
henryrawas 8:d98e2dec0f40 209 TapsSeq.push_back(tapUp);
henryrawas 8:d98e2dec0f40 210 TapsSeq.push_back(pause200ms);
henryrawas 8:d98e2dec0f40 211 TapsSeq.push_back(taps1);
henryrawas 8:d98e2dec0f40 212 TapsSeq.push_back(tapDown);
henryrawas 8:d98e2dec0f40 213 TapsSeq.push_back(tapUp);
henryrawas 8:d98e2dec0f40 214 TapsSeq.push_back(pause100ms);
henryrawas 8:d98e2dec0f40 215 TapsSeq.push_back(taps5);
henryrawas 8:d98e2dec0f40 216 TapsSeq.push_back(tapDown);
henryrawas 8:d98e2dec0f40 217 TapsSeq.push_back(tapUp);
henryrawas 8:d98e2dec0f40 218 TapsSeq.push_back(pause100ms);
henryrawas 8:d98e2dec0f40 219 TapsSeq.push_back(taps7);
henryrawas 8:d98e2dec0f40 220 TapsSeq.push_back(tapDown);
henryrawas 8:d98e2dec0f40 221 TapsSeq.push_back(tapUp);
henryrawas 8:d98e2dec0f40 222 TapsSeq.push_back(pause200ms);
henryrawas 8:d98e2dec0f40 223 TapsSeq.push_back(taps6);
henryrawas 8:d98e2dec0f40 224 TapsSeq.push_back(tapDown);
henryrawas 8:d98e2dec0f40 225 TapsSeq.push_back(tapUp);
henryrawas 8:d98e2dec0f40 226 TapsSeq.push_back(pause100ms);
henryrawas 8:d98e2dec0f40 227 TapsSeq.push_back(taps7);
henryrawas 8:d98e2dec0f40 228 TapsSeq.push_back(tapDown);
henryrawas 8:d98e2dec0f40 229 TapsSeq.push_back(tapUp);
henryrawas 8:d98e2dec0f40 230 TapsSeq.push_back(report);
henryrawas 8:d98e2dec0f40 231
henryrawas 5:36916b1c5a06 232 }