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

main.cpp@5:36916b1c5a06, 2015-12-28 (annotated)

Committer:
henryrawas
Date:
Mon Dec 28 17:19:37 2015 +0000
Revision:
5:36916b1c5a06
Parent:
4:36a4eceb1b7f
Child:
7:6723f6887d00
Working Robotarm

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jepickett 0:547ac906c46b 1 #include "mbed.h"
henryrawas 4:36a4eceb1b7f 2 #include "rtos.h"
henryrawas 4:36a4eceb1b7f 3
henryrawas 4:36a4eceb1b7f 4 #include "EthernetInterface.h"
henryrawas 4:36a4eceb1b7f 5 #include "mbed/logging.h"
henryrawas 4:36a4eceb1b7f 6 #include "mbed/mbedtime.h"
henryrawas 4:36a4eceb1b7f 7 #include <NTPClient.h>
jepickett 2:37021fb3b45b 8 #include <DynamixelBus.h>
jepickett 2:37021fb3b45b 9 #include <AX12.h>
jepickett 2:37021fb3b45b 10 #include <Terminal.h>
jepickett 2:37021fb3b45b 11 #include <vector>
henryrawas 4:36a4eceb1b7f 12 #include <RobotArm.h>
henryrawas 4:36a4eceb1b7f 13 #include <MeasureBuf.h>
henryrawas 4:36a4eceb1b7f 14 #include <IothubRobotArm.h>
henryrawas 4:36a4eceb1b7f 15 #include <ActionBuf.h>
henryrawas 5:36916b1c5a06 16 #include <Sequences.h>
henryrawas 4:36a4eceb1b7f 17
jepickett 2:37021fb3b45b 18 using namespace std;
jepickett 0:547ac906c46b 19
jepickett 2:37021fb3b45b 20 DigitalOut myled(LED_GREEN);
jepickett 2:37021fb3b45b 21 Terminal pc(USBTX, USBRX);
henryrawas 4:36a4eceb1b7f 22
henryrawas 4:36a4eceb1b7f 23 extern Terminal* RobotArmPc;
henryrawas 4:36a4eceb1b7f 24 extern Terminal* AX12Pc;
henryrawas 4:36a4eceb1b7f 25
henryrawas 5:36916b1c5a06 26 const float UpPos = 180.0f;
henryrawas 4:36a4eceb1b7f 27
henryrawas 4:36a4eceb1b7f 28
henryrawas 4:36a4eceb1b7f 29 void PushMeasurements(MeasureGroup measureGroup, int partSize, RobotArm robotArm)
henryrawas 4:36a4eceb1b7f 30 {
henryrawas 4:36a4eceb1b7f 31 vector<float> lastVals;
henryrawas 4:36a4eceb1b7f 32
henryrawas 4:36a4eceb1b7f 33 robotArm.GetArmMeasure(NM_Temperature, lastVals);
henryrawas 4:36a4eceb1b7f 34 pc.printf( "Temperatures: ");
henryrawas 4:36a4eceb1b7f 35 for( int servoIndex = 0; servoIndex < partSize; servoIndex++ )
henryrawas 4:36a4eceb1b7f 36 {
henryrawas 4:36a4eceb1b7f 37 pc.printf( "%d:%f ", servoIndex, lastVals[servoIndex]);
henryrawas 4:36a4eceb1b7f 38 }
henryrawas 4:36a4eceb1b7f 39 pc.printf( "\r\n");
henryrawas 4:36a4eceb1b7f 40 measureGroup.SetMeasure(NM_Temperature, lastVals);
henryrawas 4:36a4eceb1b7f 41 MeasureBuf.push(measureGroup);
henryrawas 4:36a4eceb1b7f 42
henryrawas 4:36a4eceb1b7f 43 robotArm.GetArmMeasure(NM_Voltage, lastVals);
henryrawas 4:36a4eceb1b7f 44 pc.printf( "Voltage: ");
henryrawas 4:36a4eceb1b7f 45 for( int servoIndex = 0; servoIndex < partSize; servoIndex++ )
henryrawas 4:36a4eceb1b7f 46 {
henryrawas 4:36a4eceb1b7f 47 pc.printf( "%d:%f ", servoIndex, lastVals[servoIndex]);
henryrawas 4:36a4eceb1b7f 48 }
henryrawas 4:36a4eceb1b7f 49 pc.printf( "\r\n");
henryrawas 4:36a4eceb1b7f 50 measureGroup.SetMeasure(NM_Voltage, lastVals);
henryrawas 4:36a4eceb1b7f 51 MeasureBuf.push(measureGroup);
henryrawas 4:36a4eceb1b7f 52
henryrawas 4:36a4eceb1b7f 53 robotArm.GetArmMeasure(NM_Degrees, lastVals);
henryrawas 4:36a4eceb1b7f 54 pc.printf( "Rotation: ");
henryrawas 4:36a4eceb1b7f 55 for( int servoIndex = 0; servoIndex < partSize; servoIndex++ )
henryrawas 4:36a4eceb1b7f 56 {
henryrawas 4:36a4eceb1b7f 57 pc.printf( "%d:%f ", servoIndex, lastVals[servoIndex]);
henryrawas 4:36a4eceb1b7f 58 }
henryrawas 4:36a4eceb1b7f 59 pc.printf( "\r\n");
henryrawas 4:36a4eceb1b7f 60 measureGroup.SetMeasure(NM_Degrees, lastVals);
henryrawas 4:36a4eceb1b7f 61 MeasureBuf.push(measureGroup);
henryrawas 4:36a4eceb1b7f 62 }
henryrawas 4:36a4eceb1b7f 63
henryrawas 4:36a4eceb1b7f 64
henryrawas 4:36a4eceb1b7f 65 int setupRealTime(void)
henryrawas 4:36a4eceb1b7f 66 {
henryrawas 4:36a4eceb1b7f 67 int result;
henryrawas 4:36a4eceb1b7f 68
henryrawas 4:36a4eceb1b7f 69 (void)printf("setupRealTime begin\r\n");
henryrawas 4:36a4eceb1b7f 70 if (EthernetInterface::connect())
henryrawas 4:36a4eceb1b7f 71 {
henryrawas 4:36a4eceb1b7f 72 (void)printf("Error initializing EthernetInterface.\r\n");
henryrawas 4:36a4eceb1b7f 73 result = __LINE__;
henryrawas 4:36a4eceb1b7f 74 }
henryrawas 4:36a4eceb1b7f 75 else
henryrawas 4:36a4eceb1b7f 76 {
henryrawas 4:36a4eceb1b7f 77 (void)printf("setupRealTime NTP begin\r\n");
henryrawas 4:36a4eceb1b7f 78 NTPClient ntp;
henryrawas 4:36a4eceb1b7f 79 if (ntp.setTime("0.pool.ntp.org") != 0)
henryrawas 4:36a4eceb1b7f 80 {
henryrawas 4:36a4eceb1b7f 81 (void)printf("Failed setting time.\r\n");
henryrawas 4:36a4eceb1b7f 82 result = __LINE__;
henryrawas 4:36a4eceb1b7f 83 }
henryrawas 4:36a4eceb1b7f 84 else
henryrawas 4:36a4eceb1b7f 85 {
henryrawas 4:36a4eceb1b7f 86 (void)printf("set time correctly!\r\n");
henryrawas 4:36a4eceb1b7f 87 result = 0;
henryrawas 4:36a4eceb1b7f 88 }
henryrawas 4:36a4eceb1b7f 89 (void)printf("setupRealTime NTP end\r\n");
henryrawas 4:36a4eceb1b7f 90 EthernetInterface::disconnect();
henryrawas 4:36a4eceb1b7f 91 }
henryrawas 4:36a4eceb1b7f 92 (void)printf("setupRealTime end\r\n");
henryrawas 4:36a4eceb1b7f 93
henryrawas 4:36a4eceb1b7f 94 return result;
henryrawas 4:36a4eceb1b7f 95 }
henryrawas 4:36a4eceb1b7f 96
henryrawas 4:36a4eceb1b7f 97 int InitEthernet()
henryrawas 4:36a4eceb1b7f 98 {
henryrawas 4:36a4eceb1b7f 99 (void)printf("Initializing mbed specific things...\r\n");
henryrawas 4:36a4eceb1b7f 100
henryrawas 4:36a4eceb1b7f 101 /* These are needed in order to initialize the time provider for Proton-C */
henryrawas 4:36a4eceb1b7f 102 mbed_log_init();
henryrawas 4:36a4eceb1b7f 103 mbedtime_init();
henryrawas 4:36a4eceb1b7f 104
henryrawas 4:36a4eceb1b7f 105 if (EthernetInterface::init())
henryrawas 4:36a4eceb1b7f 106 {
henryrawas 4:36a4eceb1b7f 107 (void)printf("Error initializing EthernetInterface.\r\n");
henryrawas 4:36a4eceb1b7f 108 return -1;
henryrawas 4:36a4eceb1b7f 109 }
henryrawas 4:36a4eceb1b7f 110
henryrawas 4:36a4eceb1b7f 111 if (setupRealTime() != 0)
henryrawas 4:36a4eceb1b7f 112 {
henryrawas 4:36a4eceb1b7f 113 (void)printf("Failed setting up real time clock\r\n");
henryrawas 4:36a4eceb1b7f 114 return -1;
henryrawas 4:36a4eceb1b7f 115 }
henryrawas 4:36a4eceb1b7f 116
henryrawas 4:36a4eceb1b7f 117 if (EthernetInterface::connect())
henryrawas 4:36a4eceb1b7f 118 {
henryrawas 4:36a4eceb1b7f 119 (void)printf("Error connecting EthernetInterface.\r\n");
henryrawas 4:36a4eceb1b7f 120 return -1;
henryrawas 4:36a4eceb1b7f 121 }
henryrawas 4:36a4eceb1b7f 122
henryrawas 4:36a4eceb1b7f 123 return 0;
henryrawas 4:36a4eceb1b7f 124 }
henryrawas 4:36a4eceb1b7f 125
henryrawas 4:36a4eceb1b7f 126
henryrawas 4:36a4eceb1b7f 127 vector<float> upPositions;
henryrawas 4:36a4eceb1b7f 128 vector<float> homePositions;
henryrawas 4:36a4eceb1b7f 129
henryrawas 4:36a4eceb1b7f 130 void MakeDemoSeq(vector<ActionSequence>& actions)
henryrawas 4:36a4eceb1b7f 131 {
henryrawas 4:36a4eceb1b7f 132 // define actions
henryrawas 5:36916b1c5a06 133 ActionSequence moveUp(SA_SetGoal, upPositions, 2500);
henryrawas 4:36a4eceb1b7f 134 ActionSequence report(SA_Status);
henryrawas 4:36a4eceb1b7f 135 ActionSequence pause(SA_Delay);
henryrawas 5:36916b1c5a06 136 pause.SetDelay(2000);
henryrawas 5:36916b1c5a06 137 ActionSequence moveDown(SA_SetGoal, homePositions, 2500);
henryrawas 4:36a4eceb1b7f 138 ActionSequence rep(SA_Repeat);
henryrawas 4:36a4eceb1b7f 139
henryrawas 4:36a4eceb1b7f 140 // add actions into a sequence
henryrawas 4:36a4eceb1b7f 141 actions.push_back(moveUp);
henryrawas 4:36a4eceb1b7f 142 actions.push_back(report);
henryrawas 4:36a4eceb1b7f 143 actions.push_back(pause);
henryrawas 4:36a4eceb1b7f 144 actions.push_back(moveDown);
henryrawas 4:36a4eceb1b7f 145 actions.push_back(report);
henryrawas 4:36a4eceb1b7f 146 actions.push_back(pause);
henryrawas 4:36a4eceb1b7f 147 actions.push_back(rep);
henryrawas 4:36a4eceb1b7f 148 }
henryrawas 4:36a4eceb1b7f 149
henryrawas 4:36a4eceb1b7f 150
henryrawas 4:36a4eceb1b7f 151 enum MainStates
henryrawas 4:36a4eceb1b7f 152 {
henryrawas 4:36a4eceb1b7f 153 MS_Idle = 0x0,
henryrawas 4:36a4eceb1b7f 154 MS_Running = 0x1,
henryrawas 4:36a4eceb1b7f 155 MS_Paused = 0x2,
henryrawas 4:36a4eceb1b7f 156 MS_Stopping = 0x3,
henryrawas 4:36a4eceb1b7f 157 MS_Error = 0x4,
henryrawas 4:36a4eceb1b7f 158 MS_CancelOne = 0x5,
henryrawas 4:36a4eceb1b7f 159 MS_CancelAll = 0x6,
henryrawas 4:36a4eceb1b7f 160 MS_CancelToM = 0x7
henryrawas 4:36a4eceb1b7f 161 };
henryrawas 4:36a4eceb1b7f 162
henryrawas 4:36a4eceb1b7f 163 Queue<vector<ActionSequence>, 16> SequenceQ;
henryrawas 5:36916b1c5a06 164 volatile MainStates MainState;
henryrawas 4:36a4eceb1b7f 165 bool RunInSequence;
henryrawas 4:36a4eceb1b7f 166 bool RunInMoveStep;
henryrawas 4:36a4eceb1b7f 167 bool RunInDelay;
henryrawas 4:36a4eceb1b7f 168 void* CancelToMatch;
henryrawas 4:36a4eceb1b7f 169
henryrawas 5:36916b1c5a06 170 osThreadId mainTid;
henryrawas 5:36916b1c5a06 171
henryrawas 5:36916b1c5a06 172 void ControlArm(const char* cmd)
henryrawas 5:36916b1c5a06 173 {
henryrawas 5:36916b1c5a06 174 if (strncmp(cmd, "pause", 5) == 0)
henryrawas 5:36916b1c5a06 175 {
henryrawas 5:36916b1c5a06 176 pc.printf( "Pause cmd\r\n");
henryrawas 5:36916b1c5a06 177 MainState = MS_Paused;
henryrawas 5:36916b1c5a06 178 osSignalSet(mainTid, AS_Action);
henryrawas 5:36916b1c5a06 179 }
henryrawas 5:36916b1c5a06 180 else if (strncmp(cmd, "resume", 6) == 0)
henryrawas 5:36916b1c5a06 181 {
henryrawas 5:36916b1c5a06 182 pc.printf( "Resume cmd\r\n");
henryrawas 5:36916b1c5a06 183 MainState = MS_Running;
henryrawas 5:36916b1c5a06 184 osSignalSet(mainTid, AS_Action);
henryrawas 5:36916b1c5a06 185 }
henryrawas 5:36916b1c5a06 186 else if (strncmp(cmd, "runupdown", 9) == 0)
henryrawas 5:36916b1c5a06 187 {
henryrawas 5:36916b1c5a06 188 pc.printf( "runupdown cmd\r\n");
henryrawas 5:36916b1c5a06 189 MainState = MS_CancelToM;
henryrawas 5:36916b1c5a06 190 CancelToMatch = &UpDownSeq;
henryrawas 5:36916b1c5a06 191 SequenceQ.put(&UpDownSeq);
henryrawas 5:36916b1c5a06 192 osSignalSet(mainTid, AS_Action);
henryrawas 5:36916b1c5a06 193 }
henryrawas 5:36916b1c5a06 194 else if (strncmp(cmd, "runuptwist", 10) == 0)
henryrawas 5:36916b1c5a06 195 {
henryrawas 5:36916b1c5a06 196 pc.printf( "runuptwist cmd\r\n");
henryrawas 5:36916b1c5a06 197 MainState = MS_CancelOne;
henryrawas 5:36916b1c5a06 198 CancelToMatch = &UpTwistSeq;
henryrawas 5:36916b1c5a06 199 SequenceQ.put(&UpTwistSeq);
henryrawas 5:36916b1c5a06 200 osSignalSet(mainTid, AS_Action);
henryrawas 5:36916b1c5a06 201 }
henryrawas 5:36916b1c5a06 202 else if (strncmp(cmd, "cancelone", 9) == 0)
henryrawas 5:36916b1c5a06 203 {
henryrawas 5:36916b1c5a06 204 pc.printf( "CancelOne cmd\r\n");
henryrawas 5:36916b1c5a06 205 MainState = MS_CancelOne;
henryrawas 5:36916b1c5a06 206 osSignalSet(mainTid, AS_Action);
henryrawas 5:36916b1c5a06 207 }
henryrawas 5:36916b1c5a06 208 }
henryrawas 4:36a4eceb1b7f 209
henryrawas 4:36a4eceb1b7f 210 // run sequence thread timer routine
henryrawas 4:36a4eceb1b7f 211 void NextSeq(void const * tid)
henryrawas 4:36a4eceb1b7f 212 {
henryrawas 4:36a4eceb1b7f 213 RunInDelay = false;
henryrawas 4:36a4eceb1b7f 214 osSignalSet((osThreadId)tid, AS_Action);
henryrawas 4:36a4eceb1b7f 215 }
henryrawas 4:36a4eceb1b7f 216
henryrawas 4:36a4eceb1b7f 217 void run()
henryrawas 4:36a4eceb1b7f 218 {
henryrawas 4:36a4eceb1b7f 219 // init robot arm
henryrawas 4:36a4eceb1b7f 220 RobotArm robotArm;
henryrawas 4:36a4eceb1b7f 221
henryrawas 4:36a4eceb1b7f 222 int partSize = robotArm.GetNumParts();
jepickett 2:37021fb3b45b 223
henryrawas 4:36a4eceb1b7f 224 for( int servoIndex = 0; servoIndex < partSize; servoIndex++)
henryrawas 4:36a4eceb1b7f 225 {
henryrawas 4:36a4eceb1b7f 226 upPositions.push_back(UpPos);
henryrawas 4:36a4eceb1b7f 227 }
henryrawas 4:36a4eceb1b7f 228
henryrawas 4:36a4eceb1b7f 229 // set running state
henryrawas 4:36a4eceb1b7f 230 MainState = MS_Idle;
henryrawas 4:36a4eceb1b7f 231 RunInSequence = false;
henryrawas 4:36a4eceb1b7f 232 RunInMoveStep = false;
henryrawas 4:36a4eceb1b7f 233 RunInDelay = false;
henryrawas 4:36a4eceb1b7f 234
henryrawas 4:36a4eceb1b7f 235 // get initial positions
henryrawas 4:36a4eceb1b7f 236 MeasureGroup measureGroup;
henryrawas 4:36a4eceb1b7f 237 robotArm.GetArmPositions(homePositions);
henryrawas 4:36a4eceb1b7f 238
henryrawas 4:36a4eceb1b7f 239 vector<float> lastVals;
henryrawas 4:36a4eceb1b7f 240
henryrawas 4:36a4eceb1b7f 241 // do inital status report
henryrawas 4:36a4eceb1b7f 242 PushMeasurements(measureGroup, partSize, robotArm);
henryrawas 4:36a4eceb1b7f 243 SendIothubMeasurements();
henryrawas 4:36a4eceb1b7f 244
henryrawas 4:36a4eceb1b7f 245 // Prepare main thread
henryrawas 5:36916b1c5a06 246 mainTid = osThreadGetId();
henryrawas 4:36a4eceb1b7f 247 int32_t signals = AS_Action;
henryrawas 4:36a4eceb1b7f 248
henryrawas 5:36916b1c5a06 249 // create sequences
henryrawas 5:36916b1c5a06 250 MakeSequences(partSize, homePositions);
henryrawas 5:36916b1c5a06 251
henryrawas 4:36a4eceb1b7f 252 // add to queue
henryrawas 5:36916b1c5a06 253 SequenceQ.put(&UpDownSeq);
henryrawas 4:36a4eceb1b7f 254
henryrawas 4:36a4eceb1b7f 255 // state for sequence
henryrawas 4:36a4eceb1b7f 256 vector<ActionSequence> *curseq = NULL;
henryrawas 4:36a4eceb1b7f 257 int curseqIx = 0;
henryrawas 4:36a4eceb1b7f 258
henryrawas 4:36a4eceb1b7f 259 // signal to run the default action for demo
henryrawas 4:36a4eceb1b7f 260 osSignalSet(mainTid, AS_Action);
henryrawas 4:36a4eceb1b7f 261
henryrawas 4:36a4eceb1b7f 262 RtosTimer delayTimer(NextSeq, osTimerOnce, (void *)osThreadGetId());
henryrawas 4:36a4eceb1b7f 263
henryrawas 4:36a4eceb1b7f 264 MainState = MS_Running;
henryrawas 4:36a4eceb1b7f 265
henryrawas 4:36a4eceb1b7f 266 while( true )
henryrawas 4:36a4eceb1b7f 267 {
henryrawas 4:36a4eceb1b7f 268 osEvent mainEvent = osSignalWait(signals, osWaitForever);
henryrawas 4:36a4eceb1b7f 269
henryrawas 4:36a4eceb1b7f 270 switch (MainState)
henryrawas 4:36a4eceb1b7f 271 {
henryrawas 4:36a4eceb1b7f 272 case MS_Idle:
henryrawas 4:36a4eceb1b7f 273 break;
henryrawas 4:36a4eceb1b7f 274
henryrawas 4:36a4eceb1b7f 275 case MS_Paused:
henryrawas 4:36a4eceb1b7f 276 pc.printf( "Paused\r\n");
henryrawas 4:36a4eceb1b7f 277 break;
henryrawas 4:36a4eceb1b7f 278
henryrawas 4:36a4eceb1b7f 279 case MS_Stopping:
henryrawas 4:36a4eceb1b7f 280 pc.printf( "Stopping\r\n");
henryrawas 4:36a4eceb1b7f 281 break;
henryrawas 4:36a4eceb1b7f 282
henryrawas 4:36a4eceb1b7f 283 case MS_Error:
henryrawas 4:36a4eceb1b7f 284 pc.printf( "Error\r\n");
henryrawas 4:36a4eceb1b7f 285 break;
henryrawas 4:36a4eceb1b7f 286
henryrawas 4:36a4eceb1b7f 287 case MS_CancelOne:
henryrawas 4:36a4eceb1b7f 288 pc.printf( "Cancel sequence\r\n");
henryrawas 4:36a4eceb1b7f 289 RunInSequence = false;
henryrawas 4:36a4eceb1b7f 290 RunInMoveStep = false;
henryrawas 4:36a4eceb1b7f 291 if (RunInDelay)
henryrawas 4:36a4eceb1b7f 292 {
henryrawas 4:36a4eceb1b7f 293 RunInDelay = false;
henryrawas 4:36a4eceb1b7f 294 delayTimer.stop();
henryrawas 4:36a4eceb1b7f 295 }
henryrawas 4:36a4eceb1b7f 296 MainState = MS_Running;
henryrawas 4:36a4eceb1b7f 297 osSignalSet(mainTid, AS_Action);
henryrawas 4:36a4eceb1b7f 298 break;
henryrawas 4:36a4eceb1b7f 299
henryrawas 4:36a4eceb1b7f 300 case MS_CancelAll:
henryrawas 4:36a4eceb1b7f 301 RunInSequence = false;
henryrawas 4:36a4eceb1b7f 302 RunInMoveStep = false;
henryrawas 4:36a4eceb1b7f 303 if (RunInDelay)
henryrawas 4:36a4eceb1b7f 304 {
henryrawas 4:36a4eceb1b7f 305 RunInDelay = false;
henryrawas 4:36a4eceb1b7f 306 delayTimer.stop();
henryrawas 4:36a4eceb1b7f 307 }
henryrawas 4:36a4eceb1b7f 308 while (1) {
henryrawas 4:36a4eceb1b7f 309 osEvent evt = SequenceQ.get(0);
henryrawas 4:36a4eceb1b7f 310 if (evt.status != osEventMessage)
henryrawas 4:36a4eceb1b7f 311 break;
henryrawas 4:36a4eceb1b7f 312 }
henryrawas 4:36a4eceb1b7f 313 MainState = MS_Running;
henryrawas 4:36a4eceb1b7f 314 break;
henryrawas 4:36a4eceb1b7f 315
henryrawas 4:36a4eceb1b7f 316 case MS_CancelToM:
henryrawas 4:36a4eceb1b7f 317 RunInSequence = false;
henryrawas 4:36a4eceb1b7f 318 RunInMoveStep = false;
henryrawas 4:36a4eceb1b7f 319 if (RunInDelay)
henryrawas 4:36a4eceb1b7f 320 {
henryrawas 4:36a4eceb1b7f 321 RunInDelay = false;
henryrawas 4:36a4eceb1b7f 322 delayTimer.stop();
henryrawas 4:36a4eceb1b7f 323 }
henryrawas 4:36a4eceb1b7f 324 while (1) {
henryrawas 4:36a4eceb1b7f 325 osEvent evt = SequenceQ.get(0);
henryrawas 4:36a4eceb1b7f 326 if (evt.status != osEventMessage)
henryrawas 4:36a4eceb1b7f 327 break;
henryrawas 4:36a4eceb1b7f 328 else if (evt.value.p == CancelToMatch)
henryrawas 4:36a4eceb1b7f 329 {
henryrawas 4:36a4eceb1b7f 330 curseq = (vector<ActionSequence> *)evt.value.p;
henryrawas 4:36a4eceb1b7f 331 curseqIx = 0;
henryrawas 4:36a4eceb1b7f 332 RunInSequence = true;
henryrawas 4:36a4eceb1b7f 333 }
henryrawas 4:36a4eceb1b7f 334 }
henryrawas 4:36a4eceb1b7f 335 MainState = MS_Running;
henryrawas 4:36a4eceb1b7f 336 osSignalSet(mainTid, AS_Action);
henryrawas 4:36a4eceb1b7f 337 break;
henryrawas 4:36a4eceb1b7f 338
henryrawas 4:36a4eceb1b7f 339 case MS_Running:
henryrawas 4:36a4eceb1b7f 340 if (RunInDelay)
henryrawas 4:36a4eceb1b7f 341 {
henryrawas 4:36a4eceb1b7f 342 // waiting for timer to fire. do nothing
henryrawas 4:36a4eceb1b7f 343 break;
henryrawas 4:36a4eceb1b7f 344 }
henryrawas 4:36a4eceb1b7f 345 if (!RunInSequence)
henryrawas 4:36a4eceb1b7f 346 {
henryrawas 4:36a4eceb1b7f 347 osEvent evt = SequenceQ.get(0);
henryrawas 4:36a4eceb1b7f 348 if (evt.status == osEventMessage)
henryrawas 4:36a4eceb1b7f 349 {
henryrawas 4:36a4eceb1b7f 350 pc.printf( "New Seq \r\n");
henryrawas 4:36a4eceb1b7f 351 curseq = (vector<ActionSequence> *)evt.value.p;
henryrawas 4:36a4eceb1b7f 352 curseqIx = 0;
henryrawas 4:36a4eceb1b7f 353 RunInSequence = true;
henryrawas 4:36a4eceb1b7f 354 RunInMoveStep = false;
henryrawas 4:36a4eceb1b7f 355 }
henryrawas 4:36a4eceb1b7f 356 }
henryrawas 4:36a4eceb1b7f 357
henryrawas 4:36a4eceb1b7f 358 if (RunInSequence)
henryrawas 4:36a4eceb1b7f 359 {
henryrawas 4:36a4eceb1b7f 360 if (RunInMoveStep)
henryrawas 4:36a4eceb1b7f 361 {
henryrawas 4:36a4eceb1b7f 362 if (robotArm.MoveArmPositionsHasNext())
henryrawas 4:36a4eceb1b7f 363 {
henryrawas 4:36a4eceb1b7f 364 //pc.printf( "Next Step\r\n");
henryrawas 4:36a4eceb1b7f 365 int delaystep = 0;
henryrawas 4:36a4eceb1b7f 366 bool ok = robotArm.MoveArmPositionsNext(delaystep);
henryrawas 4:36a4eceb1b7f 367 if (ok)
henryrawas 4:36a4eceb1b7f 368 {
henryrawas 4:36a4eceb1b7f 369 if (delaystep > 0)
henryrawas 4:36a4eceb1b7f 370 {
henryrawas 4:36a4eceb1b7f 371 RunInDelay = true;
henryrawas 4:36a4eceb1b7f 372 delayTimer.start(delaystep);
henryrawas 4:36a4eceb1b7f 373 }
henryrawas 4:36a4eceb1b7f 374 else
henryrawas 4:36a4eceb1b7f 375 osSignalSet(mainTid, AS_Action);
henryrawas 4:36a4eceb1b7f 376 }
henryrawas 4:36a4eceb1b7f 377 }
henryrawas 4:36a4eceb1b7f 378 else
henryrawas 4:36a4eceb1b7f 379 {
henryrawas 4:36a4eceb1b7f 380 pc.printf( "No more Step\r\n");
henryrawas 4:36a4eceb1b7f 381 RunInMoveStep = false;
henryrawas 4:36a4eceb1b7f 382 }
henryrawas 4:36a4eceb1b7f 383 }
henryrawas 4:36a4eceb1b7f 384 if (!RunInMoveStep)
henryrawas 4:36a4eceb1b7f 385 {
henryrawas 4:36a4eceb1b7f 386 pc.printf( "Next Seq %d\r\n", curseqIx);
henryrawas 5:36916b1c5a06 387
henryrawas 4:36a4eceb1b7f 388 if (curseq != NULL)
henryrawas 4:36a4eceb1b7f 389 {
henryrawas 5:36916b1c5a06 390 if (curseqIx >= curseq->size())
henryrawas 5:36916b1c5a06 391 {
henryrawas 5:36916b1c5a06 392 RunInSequence = false;
henryrawas 5:36916b1c5a06 393 break;
henryrawas 5:36916b1c5a06 394 }
henryrawas 5:36916b1c5a06 395
henryrawas 4:36a4eceb1b7f 396 ActionSequence aseq = (*curseq)[curseqIx];
henryrawas 4:36a4eceb1b7f 397 curseqIx++;
henryrawas 4:36a4eceb1b7f 398
henryrawas 4:36a4eceb1b7f 399 bool ok;
henryrawas 4:36a4eceb1b7f 400 switch (aseq.ActionType)
henryrawas 4:36a4eceb1b7f 401 {
henryrawas 4:36a4eceb1b7f 402 case SA_SetGoal:
henryrawas 4:36a4eceb1b7f 403 pc.printf( " - Move arm start\r\n");
henryrawas 4:36a4eceb1b7f 404 ok = robotArm.MoveArmPositionsStart(aseq.GoalVals, aseq.Ms);
henryrawas 4:36a4eceb1b7f 405 if (ok)
henryrawas 4:36a4eceb1b7f 406 {
henryrawas 4:36a4eceb1b7f 407 RunInMoveStep = true;
henryrawas 4:36a4eceb1b7f 408 osSignalSet(mainTid, AS_Action);
henryrawas 4:36a4eceb1b7f 409 }
henryrawas 4:36a4eceb1b7f 410 else
henryrawas 4:36a4eceb1b7f 411 {
henryrawas 4:36a4eceb1b7f 412 // TODO: send alert
henryrawas 4:36a4eceb1b7f 413 }
henryrawas 4:36a4eceb1b7f 414 break;
henryrawas 4:36a4eceb1b7f 415 case SA_Delay:
henryrawas 4:36a4eceb1b7f 416 pc.printf( " - Delay\r\n");
henryrawas 4:36a4eceb1b7f 417 RunInDelay = true;
henryrawas 4:36a4eceb1b7f 418 delayTimer.start(aseq.Ms);
henryrawas 4:36a4eceb1b7f 419 break;
henryrawas 4:36a4eceb1b7f 420
henryrawas 4:36a4eceb1b7f 421 case SA_Status:
henryrawas 4:36a4eceb1b7f 422 pc.printf( " - Status\r\n");
henryrawas 4:36a4eceb1b7f 423 robotArm.GetArmPositions(lastVals);
henryrawas 4:36a4eceb1b7f 424
henryrawas 4:36a4eceb1b7f 425 PushMeasurements(measureGroup, partSize, robotArm);
henryrawas 4:36a4eceb1b7f 426
henryrawas 4:36a4eceb1b7f 427 SendIothubMeasurements();
henryrawas 4:36a4eceb1b7f 428
henryrawas 4:36a4eceb1b7f 429 osSignalSet(mainTid, AS_Action);
henryrawas 4:36a4eceb1b7f 430 break;
henryrawas 4:36a4eceb1b7f 431 case SA_Repeat:
henryrawas 4:36a4eceb1b7f 432 pc.printf( " - Repeat\r\n");
henryrawas 4:36a4eceb1b7f 433 curseqIx = 0;
henryrawas 4:36a4eceb1b7f 434 osSignalSet(mainTid, AS_Action);
henryrawas 4:36a4eceb1b7f 435 break;
henryrawas 4:36a4eceb1b7f 436
henryrawas 4:36a4eceb1b7f 437 }
henryrawas 4:36a4eceb1b7f 438 }
henryrawas 4:36a4eceb1b7f 439 }
henryrawas 4:36a4eceb1b7f 440
henryrawas 4:36a4eceb1b7f 441 }
henryrawas 4:36a4eceb1b7f 442 break;
henryrawas 4:36a4eceb1b7f 443 }
henryrawas 4:36a4eceb1b7f 444
henryrawas 4:36a4eceb1b7f 445 }
henryrawas 4:36a4eceb1b7f 446 }
jepickett 0:547ac906c46b 447
jepickett 0:547ac906c46b 448 int main()
jepickett 0:547ac906c46b 449 {
jepickett 2:37021fb3b45b 450 pc.baud(115200);
jepickett 2:37021fb3b45b 451
jepickett 2:37021fb3b45b 452 pc.cls();
jepickett 2:37021fb3b45b 453 pc.foreground(Yellow);
jepickett 2:37021fb3b45b 454 pc.background(Black);
jepickett 2:37021fb3b45b 455
jepickett 2:37021fb3b45b 456 pc.locate( 0, 0 );
jepickett 2:37021fb3b45b 457 pc.printf("**********************\r\n");
jepickett 2:37021fb3b45b 458 pc.printf("RobotArmDemo start\r\n");
jepickett 2:37021fb3b45b 459 pc.printf("**********************\r\n");
jepickett 2:37021fb3b45b 460
jepickett 2:37021fb3b45b 461 pc.foreground(Teal);
jepickett 2:37021fb3b45b 462 pc.background(Black);
jepickett 2:37021fb3b45b 463
henryrawas 4:36a4eceb1b7f 464 InitEthernet();
henryrawas 4:36a4eceb1b7f 465
henryrawas 4:36a4eceb1b7f 466 // start IotHub connection
henryrawas 4:36a4eceb1b7f 467 StartIothubThread();
jepickett 2:37021fb3b45b 468
henryrawas 4:36a4eceb1b7f 469 // time delay is to allow the position encoders to come online after initial power supply event ~ 5 secs
henryrawas 4:36a4eceb1b7f 470 // and to allow IoTHub SSL connection established
henryrawas 4:36a4eceb1b7f 471 // TODO: test for connection established
henryrawas 4:36a4eceb1b7f 472 Thread::wait(15000);
henryrawas 4:36a4eceb1b7f 473
henryrawas 4:36a4eceb1b7f 474 pc.printf( "Initialization done. Ready to run. \r\n");
jepickett 2:37021fb3b45b 475
henryrawas 4:36a4eceb1b7f 476 RobotArmPc = &pc;
henryrawas 4:36a4eceb1b7f 477 AX12Pc = &pc;
jepickett 2:37021fb3b45b 478
henryrawas 4:36a4eceb1b7f 479 run();
jepickett 2:37021fb3b45b 480
henryrawas 4:36a4eceb1b7f 481 }