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@33:8b9dcbf6d8ec, 2016-02-04 (annotated)

Committer:
henryrawas
Date:
Thu Feb 04 20:10:33 2016 +0000
Revision:
33:8b9dcbf6d8ec
Parent:
24:f6aa0e64c5e1 
update libs

Who changed what in which revision?

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