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@7:6723f6887d00, 2015-12-29 (annotated)

Committer:
henryrawas
Date:
Tue Dec 29 23:31:28 2015 +0000
Revision:
7:6723f6887d00
Parent:
5:36916b1c5a06
Child:
8:d98e2dec0f40
motion block alerts, more commands

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 5:36916b1c5a06 14
henryrawas 5:36916b1c5a06 15
henryrawas 7:6723f6887d00 16 void MakeSequences(int partSize, vector<float>& startPositions)
henryrawas 5:36916b1c5a06 17 {
henryrawas 5:36916b1c5a06 18 vector<float> upPositions;
henryrawas 7:6723f6887d00 19 vector<float> downPositions;
henryrawas 7:6723f6887d00 20 vector<float> homePositions;
henryrawas 7:6723f6887d00 21 vector<float> waveUpPositions;
henryrawas 7:6723f6887d00 22 vector<float> waveDownPositions;
henryrawas 7:6723f6887d00 23 vector<float> rightPositions;
henryrawas 7:6723f6887d00 24
henryrawas 7:6723f6887d00 25 for (int partIx = 0; partIx < partSize; partIx++)
henryrawas 5:36916b1c5a06 26 {
henryrawas 5:36916b1c5a06 27 upPositions.push_back(UpPos);
henryrawas 5:36916b1c5a06 28 }
henryrawas 7:6723f6887d00 29
henryrawas 7:6723f6887d00 30 for (int partIx = 0; partIx < partSize; partIx++)
henryrawas 7:6723f6887d00 31 {
henryrawas 7:6723f6887d00 32 rightPositions.push_back(UpPos);
henryrawas 7:6723f6887d00 33 }
henryrawas 7:6723f6887d00 34
henryrawas 7:6723f6887d00 35 // define normal start position
henryrawas 7:6723f6887d00 36 homePositions.push_back(RightPos);
henryrawas 7:6723f6887d00 37 if (partSize > 3)
henryrawas 7:6723f6887d00 38 {
henryrawas 7:6723f6887d00 39 homePositions.push_back(225.0f);
henryrawas 7:6723f6887d00 40 homePositions.push_back(270.0f);
henryrawas 7:6723f6887d00 41 homePositions.push_back(135.0f);
henryrawas 7:6723f6887d00 42 for (int partIx = 4; partIx < partSize; partIx++)
henryrawas 7:6723f6887d00 43 {
henryrawas 7:6723f6887d00 44 homePositions.push_back(180.0f);
henryrawas 7:6723f6887d00 45 }
henryrawas 7:6723f6887d00 46 }
henryrawas 7:6723f6887d00 47
henryrawas 7:6723f6887d00 48 // define down position
henryrawas 7:6723f6887d00 49 downPositions.push_back(RightPos);
henryrawas 7:6723f6887d00 50 if (partSize > 3)
henryrawas 7:6723f6887d00 51 {
henryrawas 7:6723f6887d00 52 downPositions.push_back(120.0f);
henryrawas 7:6723f6887d00 53 downPositions.push_back(240.0f);
henryrawas 7:6723f6887d00 54 downPositions.push_back(150.0f);
henryrawas 7:6723f6887d00 55 for (int partIx = 4; partIx < partSize; partIx++)
henryrawas 7:6723f6887d00 56 {
henryrawas 7:6723f6887d00 57 downPositions.push_back(240.0f);
henryrawas 7:6723f6887d00 58 }
henryrawas 7:6723f6887d00 59 }
henryrawas 7:6723f6887d00 60
henryrawas 7:6723f6887d00 61 // define wave up and wave down to only move last part
henryrawas 7:6723f6887d00 62 for (int partIx = 0; partIx < partSize - 1; partIx++)
henryrawas 7:6723f6887d00 63 {
henryrawas 7:6723f6887d00 64 waveUpPositions.push_back(NoMove);
henryrawas 7:6723f6887d00 65 waveDownPositions.push_back(NoMove);
henryrawas 7:6723f6887d00 66 }
henryrawas 7:6723f6887d00 67 waveUpPositions.push_back(120.0f);
henryrawas 7:6723f6887d00 68 waveDownPositions.push_back(210.0f);
henryrawas 5:36916b1c5a06 69
henryrawas 5:36916b1c5a06 70 // define actions
henryrawas 7:6723f6887d00 71 ActionSequence moveStart(SA_SetGoal, homePositions, 1500);
henryrawas 5:36916b1c5a06 72 ActionSequence moveUp(SA_SetGoal, upPositions, 2500);
henryrawas 7:6723f6887d00 73 ActionSequence moveDown(SA_SetGoal, downPositions, 2500);
henryrawas 7:6723f6887d00 74 ActionSequence waveUp(SA_SetGoal, waveUpPositions, 1500);
henryrawas 7:6723f6887d00 75 ActionSequence waveDown(SA_SetGoal, waveDownPositions, 1500);
henryrawas 7:6723f6887d00 76
henryrawas 5:36916b1c5a06 77 ActionSequence report(SA_Status);
henryrawas 5:36916b1c5a06 78 ActionSequence pause2(SA_Delay);
henryrawas 5:36916b1c5a06 79 pause2.SetDelay(2000);
henryrawas 5:36916b1c5a06 80 ActionSequence rep(SA_Repeat);
henryrawas 5:36916b1c5a06 81
henryrawas 7:6723f6887d00 82 // add actions into StartSeq
henryrawas 7:6723f6887d00 83 StartSeq.clear();
henryrawas 7:6723f6887d00 84 StartSeq.push_back(moveStart);
henryrawas 7:6723f6887d00 85 StartSeq.push_back(report);
henryrawas 7:6723f6887d00 86
henryrawas 7:6723f6887d00 87 // add actions into WaveSeq
henryrawas 7:6723f6887d00 88 WaveSeq.clear();
henryrawas 7:6723f6887d00 89 WaveSeq.push_back(waveUp);
henryrawas 7:6723f6887d00 90 WaveSeq.push_back(report);
henryrawas 7:6723f6887d00 91 WaveSeq.push_back(waveDown);
henryrawas 7:6723f6887d00 92 WaveSeq.push_back(report);
henryrawas 7:6723f6887d00 93 WaveSeq.push_back(rep);
henryrawas 7:6723f6887d00 94
henryrawas 5:36916b1c5a06 95 // add actions into UpDownSeq
henryrawas 5:36916b1c5a06 96 UpDownSeq.clear();
henryrawas 5:36916b1c5a06 97 UpDownSeq.push_back(moveUp);
henryrawas 5:36916b1c5a06 98 UpDownSeq.push_back(report);
henryrawas 5:36916b1c5a06 99 UpDownSeq.push_back(pause2);
henryrawas 5:36916b1c5a06 100 UpDownSeq.push_back(moveDown);
henryrawas 5:36916b1c5a06 101 UpDownSeq.push_back(report);
henryrawas 5:36916b1c5a06 102 UpDownSeq.push_back(pause2);
henryrawas 5:36916b1c5a06 103 UpDownSeq.push_back(rep);
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 5:36916b1c5a06 112 vector<float>* rightPositions = new vector<float>();
henryrawas 7:6723f6887d00 113 for (int partIy = 0; partIy < partSize; partIy++)
henryrawas 5:36916b1c5a06 114 {
henryrawas 5:36916b1c5a06 115 float val = UpPos;
henryrawas 5:36916b1c5a06 116 if (partIx == partIy)
henryrawas 7:6723f6887d00 117 val = RightPos;
henryrawas 5:36916b1c5a06 118 rightPositions->push_back(val);
henryrawas 5:36916b1c5a06 119 }
henryrawas 5:36916b1c5a06 120
henryrawas 5:36916b1c5a06 121 ActionSequence* seq = new ActionSequence(SA_SetGoal, *rightPositions, 1000);
henryrawas 5:36916b1c5a06 122 UpTwistSeq.push_back(*seq);
henryrawas 5:36916b1c5a06 123 UpTwistSeq.push_back(pause2);
henryrawas 5:36916b1c5a06 124 UpTwistSeq.push_back(report);
henryrawas 5:36916b1c5a06 125 UpTwistSeq.push_back(moveUp);
henryrawas 5:36916b1c5a06 126 }
henryrawas 5:36916b1c5a06 127 UpTwistSeq.push_back(pause2);
henryrawas 5:36916b1c5a06 128 UpTwistSeq.push_back(moveDown);
henryrawas 5:36916b1c5a06 129
henryrawas 5:36916b1c5a06 130 }