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

ArmController.cpp@7:6723f6887d00, 2015-12-29 (annotated)

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
henryrawas 7:6723f6887d00 1 #include "mbed.h"
henryrawas 7:6723f6887d00 2 #include "rtos.h"
henryrawas 7:6723f6887d00 3
henryrawas 7:6723f6887d00 4 #include "EthernetInterface.h"
henryrawas 7:6723f6887d00 5 #include "mbed/logging.h"
henryrawas 7:6723f6887d00 6 #include "mbed/mbedtime.h"
henryrawas 7:6723f6887d00 7 #include <NTPClient.h>
henryrawas 7:6723f6887d00 8 #include <DynamixelBus.h>
henryrawas 7:6723f6887d00 9 #include <AX12.h>
henryrawas 7:6723f6887d00 10 #include <Terminal.h>
henryrawas 7:6723f6887d00 11 #include <vector>
henryrawas 7:6723f6887d00 12 #include <RobotArm.h>
henryrawas 7:6723f6887d00 13 #include <MeasureBuf.h>
henryrawas 7:6723f6887d00 14 #include <IothubRobotArm.h>
henryrawas 7:6723f6887d00 15 #include <ActionBuf.h>
henryrawas 7:6723f6887d00 16 #include <Sequences.h>
henryrawas 7:6723f6887d00 17
henryrawas 7:6723f6887d00 18 using namespace std;
henryrawas 7:6723f6887d00 19
henryrawas 7:6723f6887d00 20
henryrawas 7:6723f6887d00 21 void PushMeasurements(MeasureGroup measureGroup, int partSize, RobotArm robotArm)
henryrawas 7:6723f6887d00 22 {
henryrawas 7:6723f6887d00 23 vector<float> lastVals;
henryrawas 7:6723f6887d00 24 time_t seconds = time(NULL);
henryrawas 7:6723f6887d00 25
henryrawas 7:6723f6887d00 26 robotArm.GetArmMeasure(NM_Temperature, lastVals);
henryrawas 7:6723f6887d00 27 printf( "Temperatures: ");
henryrawas 7:6723f6887d00 28 for( int servoIndex = 0; servoIndex < partSize; servoIndex++ )
henryrawas 7:6723f6887d00 29 {
henryrawas 7:6723f6887d00 30 printf( "%d:%f ", servoIndex, lastVals[servoIndex]);
henryrawas 7:6723f6887d00 31 }
henryrawas 7:6723f6887d00 32 printf( "\r\n");
henryrawas 7:6723f6887d00 33 measureGroup.SetMeasure(NM_Temperature, seconds, lastVals);
henryrawas 7:6723f6887d00 34 MeasureBuf.push(measureGroup);
henryrawas 7:6723f6887d00 35
henryrawas 7:6723f6887d00 36 robotArm.GetArmMeasure(NM_Voltage, lastVals);
henryrawas 7:6723f6887d00 37 printf( "Voltage: ");
henryrawas 7:6723f6887d00 38 for( int servoIndex = 0; servoIndex < partSize; servoIndex++ )
henryrawas 7:6723f6887d00 39 {
henryrawas 7:6723f6887d00 40 printf( "%d:%f ", servoIndex, lastVals[servoIndex]);
henryrawas 7:6723f6887d00 41 }
henryrawas 7:6723f6887d00 42 printf( "\r\n");
henryrawas 7:6723f6887d00 43 measureGroup.SetMeasure(NM_Voltage, seconds, lastVals);
henryrawas 7:6723f6887d00 44 MeasureBuf.push(measureGroup);
henryrawas 7:6723f6887d00 45
henryrawas 7:6723f6887d00 46 robotArm.GetArmMeasure(NM_Degrees, lastVals);
henryrawas 7:6723f6887d00 47 printf( "Rotation: ");
henryrawas 7:6723f6887d00 48 for( int servoIndex = 0; servoIndex < partSize; servoIndex++ )
henryrawas 7:6723f6887d00 49 {
henryrawas 7:6723f6887d00 50 printf( "%d:%f ", servoIndex, lastVals[servoIndex]);
henryrawas 7:6723f6887d00 51 }
henryrawas 7:6723f6887d00 52 printf( "\r\n");
henryrawas 7:6723f6887d00 53 measureGroup.SetMeasure(NM_Degrees, seconds, lastVals);
henryrawas 7:6723f6887d00 54 MeasureBuf.push(measureGroup);
henryrawas 7:6723f6887d00 55 }
henryrawas 7:6723f6887d00 56
henryrawas 7:6723f6887d00 57 void PushAlert(int severity, char* msg, char* atype)
henryrawas 7:6723f6887d00 58 {
henryrawas 7:6723f6887d00 59 Alert alert;
henryrawas 7:6723f6887d00 60 time_t seconds = time(NULL);
henryrawas 7:6723f6887d00 61
henryrawas 7:6723f6887d00 62 alert.SetAlert(severity, seconds, msg, atype);
henryrawas 7:6723f6887d00 63 AlertBuf.push(alert);
henryrawas 7:6723f6887d00 64 }
henryrawas 7:6723f6887d00 65
henryrawas 7:6723f6887d00 66 void PushPositionAlert(int severity, int partIx, float pos)
henryrawas 7:6723f6887d00 67 {
henryrawas 7:6723f6887d00 68 Alert alert;
henryrawas 7:6723f6887d00 69 time_t seconds = time(NULL);
henryrawas 7:6723f6887d00 70
henryrawas 7:6723f6887d00 71 alert.SetPositionAlert(severity, seconds, partIx, pos);
henryrawas 7:6723f6887d00 72 AlertBuf.push(alert);
henryrawas 7:6723f6887d00 73 }
henryrawas 7:6723f6887d00 74
henryrawas 7:6723f6887d00 75
henryrawas 7:6723f6887d00 76
henryrawas 7:6723f6887d00 77 vector<float> initPositions;
henryrawas 7:6723f6887d00 78
henryrawas 7:6723f6887d00 79
henryrawas 7:6723f6887d00 80 enum MainStates
henryrawas 7:6723f6887d00 81 {
henryrawas 7:6723f6887d00 82 MS_Idle = 0x0,
henryrawas 7:6723f6887d00 83 MS_Running = 0x1,
henryrawas 7:6723f6887d00 84 MS_Paused = 0x2,
henryrawas 7:6723f6887d00 85 MS_Stopping = 0x3,
henryrawas 7:6723f6887d00 86 MS_Error = 0x4,
henryrawas 7:6723f6887d00 87 MS_CancelOne = 0x5,
henryrawas 7:6723f6887d00 88 MS_CancelAll = 0x6,
henryrawas 7:6723f6887d00 89 MS_CancelToM = 0x7
henryrawas 7:6723f6887d00 90 };
henryrawas 7:6723f6887d00 91
henryrawas 7:6723f6887d00 92 Queue<vector<ActionSequence>, 16> SequenceQ;
henryrawas 7:6723f6887d00 93 volatile MainStates MainState;
henryrawas 7:6723f6887d00 94 bool RunInSequence;
henryrawas 7:6723f6887d00 95 bool RunInMoveStep;
henryrawas 7:6723f6887d00 96 bool RunInDelay;
henryrawas 7:6723f6887d00 97 void* CancelToMatch;
henryrawas 7:6723f6887d00 98
henryrawas 7:6723f6887d00 99 osThreadId mainTid;
henryrawas 7:6723f6887d00 100
henryrawas 7:6723f6887d00 101 void ControlArm(const char* cmd)
henryrawas 7:6723f6887d00 102 {
henryrawas 7:6723f6887d00 103 if (strncmp(cmd, "pause", 5) == 0)
henryrawas 7:6723f6887d00 104 {
henryrawas 7:6723f6887d00 105 printf( "Pause cmd\r\n");
henryrawas 7:6723f6887d00 106 MainState = MS_Paused;
henryrawas 7:6723f6887d00 107 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 108 }
henryrawas 7:6723f6887d00 109 else if (strncmp(cmd, "resume", 6) == 0)
henryrawas 7:6723f6887d00 110 {
henryrawas 7:6723f6887d00 111 printf( "Resume cmd\r\n");
henryrawas 7:6723f6887d00 112 MainState = MS_Running;
henryrawas 7:6723f6887d00 113 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 114 }
henryrawas 7:6723f6887d00 115 else if (strncmp(cmd, "runupdown", 9) == 0)
henryrawas 7:6723f6887d00 116 {
henryrawas 7:6723f6887d00 117 printf( "Runupdown cmd\r\n");
henryrawas 7:6723f6887d00 118 MainState = MS_CancelToM;
henryrawas 7:6723f6887d00 119 CancelToMatch = &UpDownSeq;
henryrawas 7:6723f6887d00 120 SequenceQ.put(&UpDownSeq);
henryrawas 7:6723f6887d00 121 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 122 }
henryrawas 7:6723f6887d00 123 else if (strncmp(cmd, "runuptwist", 10) == 0)
henryrawas 7:6723f6887d00 124 {
henryrawas 7:6723f6887d00 125 printf( "Runuptwist cmd\r\n");
henryrawas 7:6723f6887d00 126 MainState = MS_CancelToM;
henryrawas 7:6723f6887d00 127 CancelToMatch = &UpTwistSeq;
henryrawas 7:6723f6887d00 128 SequenceQ.put(&UpTwistSeq);
henryrawas 7:6723f6887d00 129 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 130 }
henryrawas 7:6723f6887d00 131 else if (strncmp(cmd, "runhome", 10) == 0)
henryrawas 7:6723f6887d00 132 {
henryrawas 7:6723f6887d00 133 printf( "Runhome cmd\r\n");
henryrawas 7:6723f6887d00 134 MainState = MS_CancelToM;
henryrawas 7:6723f6887d00 135 CancelToMatch = &StartSeq;
henryrawas 7:6723f6887d00 136 SequenceQ.put(&StartSeq);
henryrawas 7:6723f6887d00 137 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 138 }
henryrawas 7:6723f6887d00 139 else if (strncmp(cmd, "runwave", 10) == 0)
henryrawas 7:6723f6887d00 140 {
henryrawas 7:6723f6887d00 141 printf( "Runwave cmd\r\n");
henryrawas 7:6723f6887d00 142 MainState = MS_CancelToM;
henryrawas 7:6723f6887d00 143 CancelToMatch = &WaveSeq;
henryrawas 7:6723f6887d00 144 SequenceQ.put(&WaveSeq);
henryrawas 7:6723f6887d00 145 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 146 }
henryrawas 7:6723f6887d00 147 else if (strncmp(cmd, "cancelone", 9) == 0)
henryrawas 7:6723f6887d00 148 {
henryrawas 7:6723f6887d00 149 printf( "CancelOne cmd\r\n");
henryrawas 7:6723f6887d00 150 MainState = MS_CancelOne;
henryrawas 7:6723f6887d00 151 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 152 }
henryrawas 7:6723f6887d00 153 }
henryrawas 7:6723f6887d00 154
henryrawas 7:6723f6887d00 155 // run sequence thread timer routine
henryrawas 7:6723f6887d00 156 void NextSeq(void const * tid)
henryrawas 7:6723f6887d00 157 {
henryrawas 7:6723f6887d00 158 RunInDelay = false;
henryrawas 7:6723f6887d00 159 osSignalSet((osThreadId)tid, AS_Action);
henryrawas 7:6723f6887d00 160 }
henryrawas 7:6723f6887d00 161
henryrawas 7:6723f6887d00 162 void RunController()
henryrawas 7:6723f6887d00 163 {
henryrawas 7:6723f6887d00 164 // init robot arm
henryrawas 7:6723f6887d00 165 RobotArm robotArm;
henryrawas 7:6723f6887d00 166
henryrawas 7:6723f6887d00 167 int partSize = robotArm.GetNumParts();
henryrawas 7:6723f6887d00 168
henryrawas 7:6723f6887d00 169
henryrawas 7:6723f6887d00 170 // set running state
henryrawas 7:6723f6887d00 171 MainState = MS_Idle;
henryrawas 7:6723f6887d00 172 RunInSequence = false;
henryrawas 7:6723f6887d00 173 RunInMoveStep = false;
henryrawas 7:6723f6887d00 174 RunInDelay = false;
henryrawas 7:6723f6887d00 175
henryrawas 7:6723f6887d00 176 // get initial positions
henryrawas 7:6723f6887d00 177 MeasureGroup measureGroup;
henryrawas 7:6723f6887d00 178 robotArm.GetArmPositions(initPositions);
henryrawas 7:6723f6887d00 179
henryrawas 7:6723f6887d00 180 vector<float> lastVals;
henryrawas 7:6723f6887d00 181
henryrawas 7:6723f6887d00 182 // do inital status report
henryrawas 7:6723f6887d00 183 PushMeasurements(measureGroup, partSize, robotArm);
henryrawas 7:6723f6887d00 184 SendIothubData();
henryrawas 7:6723f6887d00 185
henryrawas 7:6723f6887d00 186 // Prepare main thread
henryrawas 7:6723f6887d00 187 mainTid = osThreadGetId();
henryrawas 7:6723f6887d00 188 int32_t signals = AS_Action;
henryrawas 7:6723f6887d00 189
henryrawas 7:6723f6887d00 190 // create sequences
henryrawas 7:6723f6887d00 191 MakeSequences(partSize, initPositions);
henryrawas 7:6723f6887d00 192
henryrawas 7:6723f6887d00 193 // add to queue
henryrawas 7:6723f6887d00 194 SequenceQ.put(&StartSeq);
henryrawas 7:6723f6887d00 195
henryrawas 7:6723f6887d00 196 // state for sequence
henryrawas 7:6723f6887d00 197 vector<ActionSequence> *curseq = NULL;
henryrawas 7:6723f6887d00 198 int curseqIx = 0;
henryrawas 7:6723f6887d00 199
henryrawas 7:6723f6887d00 200 // signal to run the default action for demo
henryrawas 7:6723f6887d00 201 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 202
henryrawas 7:6723f6887d00 203 RtosTimer delayTimer(NextSeq, osTimerOnce, (void *)osThreadGetId());
henryrawas 7:6723f6887d00 204
henryrawas 7:6723f6887d00 205 MainState = MS_Running;
henryrawas 7:6723f6887d00 206
henryrawas 7:6723f6887d00 207 while( true )
henryrawas 7:6723f6887d00 208 {
henryrawas 7:6723f6887d00 209 osEvent mainEvent = osSignalWait(signals, osWaitForever);
henryrawas 7:6723f6887d00 210
henryrawas 7:6723f6887d00 211 switch (MainState)
henryrawas 7:6723f6887d00 212 {
henryrawas 7:6723f6887d00 213 case MS_Idle:
henryrawas 7:6723f6887d00 214 break;
henryrawas 7:6723f6887d00 215
henryrawas 7:6723f6887d00 216 case MS_Paused:
henryrawas 7:6723f6887d00 217 printf( "Paused\r\n");
henryrawas 7:6723f6887d00 218 break;
henryrawas 7:6723f6887d00 219
henryrawas 7:6723f6887d00 220 case MS_Stopping:
henryrawas 7:6723f6887d00 221 printf( "Stopping\r\n");
henryrawas 7:6723f6887d00 222 break;
henryrawas 7:6723f6887d00 223
henryrawas 7:6723f6887d00 224 case MS_Error:
henryrawas 7:6723f6887d00 225 printf( "Error\r\n");
henryrawas 7:6723f6887d00 226 break;
henryrawas 7:6723f6887d00 227
henryrawas 7:6723f6887d00 228 case MS_CancelOne:
henryrawas 7:6723f6887d00 229 printf( "Cancel sequence\r\n");
henryrawas 7:6723f6887d00 230 RunInSequence = false;
henryrawas 7:6723f6887d00 231 RunInMoveStep = false;
henryrawas 7:6723f6887d00 232 if (RunInDelay)
henryrawas 7:6723f6887d00 233 {
henryrawas 7:6723f6887d00 234 RunInDelay = false;
henryrawas 7:6723f6887d00 235 delayTimer.stop();
henryrawas 7:6723f6887d00 236 }
henryrawas 7:6723f6887d00 237 MainState = MS_Running;
henryrawas 7:6723f6887d00 238 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 239 break;
henryrawas 7:6723f6887d00 240
henryrawas 7:6723f6887d00 241 case MS_CancelAll:
henryrawas 7:6723f6887d00 242 RunInSequence = false;
henryrawas 7:6723f6887d00 243 RunInMoveStep = false;
henryrawas 7:6723f6887d00 244 if (RunInDelay)
henryrawas 7:6723f6887d00 245 {
henryrawas 7:6723f6887d00 246 RunInDelay = false;
henryrawas 7:6723f6887d00 247 delayTimer.stop();
henryrawas 7:6723f6887d00 248 }
henryrawas 7:6723f6887d00 249 while (1) {
henryrawas 7:6723f6887d00 250 osEvent evt = SequenceQ.get(0);
henryrawas 7:6723f6887d00 251 if (evt.status != osEventMessage)
henryrawas 7:6723f6887d00 252 break;
henryrawas 7:6723f6887d00 253 }
henryrawas 7:6723f6887d00 254 MainState = MS_Running;
henryrawas 7:6723f6887d00 255 break;
henryrawas 7:6723f6887d00 256
henryrawas 7:6723f6887d00 257 case MS_CancelToM:
henryrawas 7:6723f6887d00 258 RunInSequence = false;
henryrawas 7:6723f6887d00 259 RunInMoveStep = false;
henryrawas 7:6723f6887d00 260 if (RunInDelay)
henryrawas 7:6723f6887d00 261 {
henryrawas 7:6723f6887d00 262 RunInDelay = false;
henryrawas 7:6723f6887d00 263 delayTimer.stop();
henryrawas 7:6723f6887d00 264 }
henryrawas 7:6723f6887d00 265 while (1) {
henryrawas 7:6723f6887d00 266 osEvent evt = SequenceQ.get(0);
henryrawas 7:6723f6887d00 267 if (evt.status != osEventMessage)
henryrawas 7:6723f6887d00 268 break;
henryrawas 7:6723f6887d00 269 else if (evt.value.p == CancelToMatch)
henryrawas 7:6723f6887d00 270 {
henryrawas 7:6723f6887d00 271 curseq = (vector<ActionSequence> *)evt.value.p;
henryrawas 7:6723f6887d00 272 curseqIx = 0;
henryrawas 7:6723f6887d00 273 RunInSequence = true;
henryrawas 7:6723f6887d00 274 }
henryrawas 7:6723f6887d00 275 }
henryrawas 7:6723f6887d00 276 MainState = MS_Running;
henryrawas 7:6723f6887d00 277 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 278 break;
henryrawas 7:6723f6887d00 279
henryrawas 7:6723f6887d00 280 case MS_Running:
henryrawas 7:6723f6887d00 281 if (RunInDelay)
henryrawas 7:6723f6887d00 282 {
henryrawas 7:6723f6887d00 283 // waiting for timer to fire. do nothing
henryrawas 7:6723f6887d00 284 break;
henryrawas 7:6723f6887d00 285 }
henryrawas 7:6723f6887d00 286 if (!RunInSequence)
henryrawas 7:6723f6887d00 287 {
henryrawas 7:6723f6887d00 288 osEvent evt = SequenceQ.get(0);
henryrawas 7:6723f6887d00 289 if (evt.status == osEventMessage)
henryrawas 7:6723f6887d00 290 {
henryrawas 7:6723f6887d00 291 printf( "New Seq \r\n");
henryrawas 7:6723f6887d00 292 curseq = (vector<ActionSequence> *)evt.value.p;
henryrawas 7:6723f6887d00 293 curseqIx = 0;
henryrawas 7:6723f6887d00 294 RunInSequence = true;
henryrawas 7:6723f6887d00 295 RunInMoveStep = false;
henryrawas 7:6723f6887d00 296 }
henryrawas 7:6723f6887d00 297 }
henryrawas 7:6723f6887d00 298
henryrawas 7:6723f6887d00 299 if (RunInSequence)
henryrawas 7:6723f6887d00 300 {
henryrawas 7:6723f6887d00 301 if (RunInMoveStep)
henryrawas 7:6723f6887d00 302 {
henryrawas 7:6723f6887d00 303 if (!robotArm.MoveArmPositionTest())
henryrawas 7:6723f6887d00 304 {
henryrawas 7:6723f6887d00 305 // report position error
henryrawas 7:6723f6887d00 306 int ix = robotArm.GetLastErrorPart();
henryrawas 7:6723f6887d00 307 float diff = robotArm.GetLastPosDiff();
henryrawas 7:6723f6887d00 308 printf( "Position error detected part %d, diff %f \r\n", ix, diff);
henryrawas 7:6723f6887d00 309 PushPositionAlert(AS_Error, ix, diff);
henryrawas 7:6723f6887d00 310 MainState = MS_Error;
henryrawas 7:6723f6887d00 311 SendIothubData();
henryrawas 7:6723f6887d00 312 break;
henryrawas 7:6723f6887d00 313 }
henryrawas 7:6723f6887d00 314 if (robotArm.MoveArmPositionsHasNext())
henryrawas 7:6723f6887d00 315 {
henryrawas 7:6723f6887d00 316 //printf( "Next Step\r\n");
henryrawas 7:6723f6887d00 317 int delaystep = 0;
henryrawas 7:6723f6887d00 318 bool ok = robotArm.MoveArmPositionsNext();
henryrawas 7:6723f6887d00 319 if (ok)
henryrawas 7:6723f6887d00 320 {
henryrawas 7:6723f6887d00 321 robotArm.MoveArmPositionsDelay(delaystep);
henryrawas 7:6723f6887d00 322 if (delaystep > 0)
henryrawas 7:6723f6887d00 323 {
henryrawas 7:6723f6887d00 324 RunInDelay = true;
henryrawas 7:6723f6887d00 325 delayTimer.start(delaystep);
henryrawas 7:6723f6887d00 326 }
henryrawas 7:6723f6887d00 327 else
henryrawas 7:6723f6887d00 328 {
henryrawas 7:6723f6887d00 329 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 330 }
henryrawas 7:6723f6887d00 331 }
henryrawas 7:6723f6887d00 332 else
henryrawas 7:6723f6887d00 333 {
henryrawas 7:6723f6887d00 334 // report HW error
henryrawas 7:6723f6887d00 335 // int partix = robotArm.GetLastErrorPart();
henryrawas 7:6723f6887d00 336 // int errCode = robotArm.GetLastError();
henryrawas 7:6723f6887d00 337 // send alert
henryrawas 7:6723f6887d00 338 MainState = MS_Error;
henryrawas 7:6723f6887d00 339 }
henryrawas 7:6723f6887d00 340 }
henryrawas 7:6723f6887d00 341 else
henryrawas 7:6723f6887d00 342 {
henryrawas 7:6723f6887d00 343 printf( "No more Step\r\n");
henryrawas 7:6723f6887d00 344 RunInMoveStep = false;
henryrawas 7:6723f6887d00 345 }
henryrawas 7:6723f6887d00 346 }
henryrawas 7:6723f6887d00 347 if (!RunInMoveStep)
henryrawas 7:6723f6887d00 348 {
henryrawas 7:6723f6887d00 349 printf( "Next Seq %d\r\n", curseqIx);
henryrawas 7:6723f6887d00 350
henryrawas 7:6723f6887d00 351 if (curseq != NULL)
henryrawas 7:6723f6887d00 352 {
henryrawas 7:6723f6887d00 353 if (curseqIx >= curseq->size())
henryrawas 7:6723f6887d00 354 {
henryrawas 7:6723f6887d00 355 RunInSequence = false;
henryrawas 7:6723f6887d00 356 break;
henryrawas 7:6723f6887d00 357 }
henryrawas 7:6723f6887d00 358
henryrawas 7:6723f6887d00 359 ActionSequence aseq = (*curseq)[curseqIx];
henryrawas 7:6723f6887d00 360 curseqIx++;
henryrawas 7:6723f6887d00 361
henryrawas 7:6723f6887d00 362 bool ok;
henryrawas 7:6723f6887d00 363 switch (aseq.ActionType)
henryrawas 7:6723f6887d00 364 {
henryrawas 7:6723f6887d00 365 case SA_SetGoal:
henryrawas 7:6723f6887d00 366 printf( " - Move arm start\r\n");
henryrawas 7:6723f6887d00 367 ok = robotArm.MoveArmPositionsStart(aseq.GoalVals, aseq.Ms);
henryrawas 7:6723f6887d00 368 if (ok)
henryrawas 7:6723f6887d00 369 {
henryrawas 7:6723f6887d00 370 RunInMoveStep = true;
henryrawas 7:6723f6887d00 371 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 372 }
henryrawas 7:6723f6887d00 373 else
henryrawas 7:6723f6887d00 374 {
henryrawas 7:6723f6887d00 375 // TODO: send alert
henryrawas 7:6723f6887d00 376 }
henryrawas 7:6723f6887d00 377 break;
henryrawas 7:6723f6887d00 378 case SA_Delay:
henryrawas 7:6723f6887d00 379 printf( " - Delay\r\n");
henryrawas 7:6723f6887d00 380 RunInDelay = true;
henryrawas 7:6723f6887d00 381 delayTimer.start(aseq.Ms);
henryrawas 7:6723f6887d00 382 break;
henryrawas 7:6723f6887d00 383
henryrawas 7:6723f6887d00 384 case SA_Status:
henryrawas 7:6723f6887d00 385 printf( " - Status\r\n");
henryrawas 7:6723f6887d00 386 robotArm.GetArmPositions(lastVals);
henryrawas 7:6723f6887d00 387
henryrawas 7:6723f6887d00 388 PushMeasurements(measureGroup, partSize, robotArm);
henryrawas 7:6723f6887d00 389
henryrawas 7:6723f6887d00 390 SendIothubData();
henryrawas 7:6723f6887d00 391
henryrawas 7:6723f6887d00 392 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 393 break;
henryrawas 7:6723f6887d00 394 case SA_Repeat:
henryrawas 7:6723f6887d00 395 printf( " - Repeat\r\n");
henryrawas 7:6723f6887d00 396 curseqIx = 0;
henryrawas 7:6723f6887d00 397 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 398 break;
henryrawas 7:6723f6887d00 399
henryrawas 7:6723f6887d00 400 }
henryrawas 7:6723f6887d00 401 }
henryrawas 7:6723f6887d00 402 }
henryrawas 7:6723f6887d00 403
henryrawas 7:6723f6887d00 404 }
henryrawas 7:6723f6887d00 405 break;
henryrawas 7:6723f6887d00 406 }
henryrawas 7:6723f6887d00 407
henryrawas 7:6723f6887d00 408 }
henryrawas 7:6723f6887d00 409 }
henryrawas 7:6723f6887d00 410