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@10:9b21566a5ddb, 2016-01-06 (annotated)

Committer:
henryrawas
Date:
Wed Jan 06 00:58:41 2016 +0000
Revision:
10:9b21566a5ddb
Parent:
9:a0fb6c370dbb
Child:
11:3a2e6eb9fbb8
Send status every minute. Add stress sequence. Use LED colors

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 10:9b21566a5ddb 17 #include <ControllerIo.h>
henryrawas 7:6723f6887d00 18
henryrawas 7:6723f6887d00 19 using namespace std;
henryrawas 7:6723f6887d00 20
henryrawas 10:9b21566a5ddb 21
henryrawas 9:a0fb6c370dbb 22 vector<float> initPositions;
henryrawas 9:a0fb6c370dbb 23
henryrawas 9:a0fb6c370dbb 24
henryrawas 9:a0fb6c370dbb 25 enum MainStates
henryrawas 9:a0fb6c370dbb 26 {
henryrawas 9:a0fb6c370dbb 27 MS_Idle = 0x0,
henryrawas 9:a0fb6c370dbb 28 MS_Running = 0x1,
henryrawas 9:a0fb6c370dbb 29 MS_Paused = 0x2,
henryrawas 9:a0fb6c370dbb 30 MS_Stopping = 0x3,
henryrawas 9:a0fb6c370dbb 31 MS_Error = 0x4,
henryrawas 9:a0fb6c370dbb 32 MS_CancelOne = 0x5,
henryrawas 9:a0fb6c370dbb 33 MS_CancelAll = 0x6,
henryrawas 9:a0fb6c370dbb 34 MS_CancelToM = 0x7
henryrawas 9:a0fb6c370dbb 35 };
henryrawas 9:a0fb6c370dbb 36
henryrawas 10:9b21566a5ddb 37 // try to send some status every minute
henryrawas 10:9b21566a5ddb 38 #define IDLESTATUSTO 60000
henryrawas 10:9b21566a5ddb 39 Timer IdleTimer;
henryrawas 10:9b21566a5ddb 40 int LastSendMs = 0;
henryrawas 10:9b21566a5ddb 41
henryrawas 9:a0fb6c370dbb 42 Queue<vector<ActionSequence>, 16> SequenceQ;
henryrawas 9:a0fb6c370dbb 43 volatile MainStates MainState;
henryrawas 9:a0fb6c370dbb 44 bool RunInSequence;
henryrawas 9:a0fb6c370dbb 45 bool RunInMoveStep;
henryrawas 9:a0fb6c370dbb 46 bool RunInDelay;
henryrawas 9:a0fb6c370dbb 47 void* CancelToMatch;
henryrawas 9:a0fb6c370dbb 48
henryrawas 9:a0fb6c370dbb 49 osThreadId mainTid;
henryrawas 9:a0fb6c370dbb 50
henryrawas 9:a0fb6c370dbb 51
henryrawas 8:d98e2dec0f40 52 void DispMeasure(char* label, int partSize, vector<float>& vals)
henryrawas 8:d98e2dec0f40 53 {
henryrawas 8:d98e2dec0f40 54 printf("%s: ", label);
henryrawas 8:d98e2dec0f40 55 for (int ix = 0; ix < partSize; ix++)
henryrawas 8:d98e2dec0f40 56 {
henryrawas 8:d98e2dec0f40 57 printf("%d:%f ", ix, vals[ix]);
henryrawas 8:d98e2dec0f40 58 }
henryrawas 8:d98e2dec0f40 59 printf("\r\n");
henryrawas 8:d98e2dec0f40 60
henryrawas 8:d98e2dec0f40 61 }
henryrawas 7:6723f6887d00 62
henryrawas 7:6723f6887d00 63
henryrawas 7:6723f6887d00 64 void PushAlert(int severity, char* msg, char* atype)
henryrawas 7:6723f6887d00 65 {
henryrawas 7:6723f6887d00 66 Alert alert;
henryrawas 7:6723f6887d00 67 time_t seconds = time(NULL);
henryrawas 7:6723f6887d00 68
henryrawas 7:6723f6887d00 69 alert.SetAlert(severity, seconds, msg, atype);
henryrawas 7:6723f6887d00 70 AlertBuf.push(alert);
henryrawas 8:d98e2dec0f40 71 SendIothubData();
henryrawas 7:6723f6887d00 72 }
henryrawas 7:6723f6887d00 73
henryrawas 7:6723f6887d00 74 void PushPositionAlert(int severity, int partIx, float pos)
henryrawas 7:6723f6887d00 75 {
henryrawas 7:6723f6887d00 76 Alert alert;
henryrawas 7:6723f6887d00 77 time_t seconds = time(NULL);
henryrawas 7:6723f6887d00 78
henryrawas 10:9b21566a5ddb 79 ShowLedRed();
henryrawas 7:6723f6887d00 80 alert.SetPositionAlert(severity, seconds, partIx, pos);
henryrawas 7:6723f6887d00 81 AlertBuf.push(alert);
henryrawas 8:d98e2dec0f40 82 SendIothubData();
henryrawas 7:6723f6887d00 83 }
henryrawas 7:6723f6887d00 84
henryrawas 8:d98e2dec0f40 85 void PushHardwareAlert(int severity, int partIx, int code)
henryrawas 8:d98e2dec0f40 86 {
henryrawas 8:d98e2dec0f40 87 Alert alert;
henryrawas 8:d98e2dec0f40 88 time_t seconds = time(NULL);
henryrawas 8:d98e2dec0f40 89
henryrawas 10:9b21566a5ddb 90 ShowLedRed();
henryrawas 8:d98e2dec0f40 91 alert.SetHardwareAlert(severity, seconds, partIx, code);
henryrawas 8:d98e2dec0f40 92 AlertBuf.push(alert);
henryrawas 8:d98e2dec0f40 93 SendIothubData();
henryrawas 8:d98e2dec0f40 94 }
henryrawas 7:6723f6887d00 95
henryrawas 8:d98e2dec0f40 96 void PushTemperatureAlert(int severity, int partIx, float temp)
henryrawas 8:d98e2dec0f40 97 {
henryrawas 8:d98e2dec0f40 98 Alert alert;
henryrawas 8:d98e2dec0f40 99 time_t seconds = time(NULL);
henryrawas 8:d98e2dec0f40 100
henryrawas 10:9b21566a5ddb 101 ShowLedRed();
henryrawas 8:d98e2dec0f40 102 alert.SetTemperatureAlert(severity, seconds, partIx, temp);
henryrawas 8:d98e2dec0f40 103 AlertBuf.push(alert);
henryrawas 8:d98e2dec0f40 104 SendIothubData();
henryrawas 8:d98e2dec0f40 105 }
henryrawas 7:6723f6887d00 106
henryrawas 8:d98e2dec0f40 107 // send alert if temperature is above 69C and return true
henryrawas 8:d98e2dec0f40 108 #define MaxTemp 69
henryrawas 8:d98e2dec0f40 109 bool TestTemperature(vector<float>& vals)
henryrawas 8:d98e2dec0f40 110 {
henryrawas 8:d98e2dec0f40 111 bool err = false;
henryrawas 8:d98e2dec0f40 112
henryrawas 8:d98e2dec0f40 113 for (int ix = 0; ix < vals.size(); ix++)
henryrawas 8:d98e2dec0f40 114 {
henryrawas 8:d98e2dec0f40 115 if (vals[ix] > MaxTemp)
henryrawas 8:d98e2dec0f40 116 {
henryrawas 8:d98e2dec0f40 117 printf("Temperature threshold exceeded for part %d, temp %f \r\n", ix, vals[ix]);
henryrawas 8:d98e2dec0f40 118 PushTemperatureAlert(AS_Error, ix, vals[ix]);
henryrawas 8:d98e2dec0f40 119 MainState = MS_Error;
henryrawas 8:d98e2dec0f40 120 err = true;
henryrawas 8:d98e2dec0f40 121 }
henryrawas 8:d98e2dec0f40 122 }
henryrawas 8:d98e2dec0f40 123 return err;
henryrawas 8:d98e2dec0f40 124 }
henryrawas 8:d98e2dec0f40 125
henryrawas 10:9b21566a5ddb 126
henryrawas 9:a0fb6c370dbb 127 void PushMeasurements(MeasureGroup measureGroup, int partSize, RobotArm robotArm)
henryrawas 9:a0fb6c370dbb 128 {
henryrawas 9:a0fb6c370dbb 129 vector<float> lastVals;
henryrawas 9:a0fb6c370dbb 130 time_t seconds = time(NULL);
henryrawas 10:9b21566a5ddb 131 LastSendMs = (int)IdleTimer.read_ms();
henryrawas 10:9b21566a5ddb 132
henryrawas 9:a0fb6c370dbb 133 bool ok = true;
henryrawas 9:a0fb6c370dbb 134
henryrawas 9:a0fb6c370dbb 135 ok = robotArm.GetArmMeasure(NM_Temperature, lastVals);
henryrawas 9:a0fb6c370dbb 136 DispMeasure("Temperatures", partSize, lastVals);
henryrawas 9:a0fb6c370dbb 137 measureGroup.SetMeasure(NM_Temperature, seconds, lastVals);
henryrawas 9:a0fb6c370dbb 138 MeasureBuf.push(measureGroup);
henryrawas 9:a0fb6c370dbb 139
henryrawas 9:a0fb6c370dbb 140 if (ok)
henryrawas 9:a0fb6c370dbb 141 {
henryrawas 9:a0fb6c370dbb 142 ok = robotArm.GetArmMeasure(NM_Voltage, lastVals);
henryrawas 9:a0fb6c370dbb 143 DispMeasure("Voltage", partSize, lastVals);
henryrawas 9:a0fb6c370dbb 144 measureGroup.SetMeasure(NM_Voltage, seconds, lastVals);
henryrawas 9:a0fb6c370dbb 145 MeasureBuf.push(measureGroup);
henryrawas 9:a0fb6c370dbb 146 }
henryrawas 9:a0fb6c370dbb 147
henryrawas 9:a0fb6c370dbb 148 if (ok)
henryrawas 9:a0fb6c370dbb 149 {
henryrawas 9:a0fb6c370dbb 150 ok = robotArm.GetArmMeasure(NM_Degrees, lastVals);
henryrawas 9:a0fb6c370dbb 151 DispMeasure("Rotation", partSize, lastVals);
henryrawas 9:a0fb6c370dbb 152 measureGroup.SetMeasure(NM_Degrees, seconds, lastVals);
henryrawas 9:a0fb6c370dbb 153 MeasureBuf.push(measureGroup);
henryrawas 9:a0fb6c370dbb 154 }
henryrawas 9:a0fb6c370dbb 155
henryrawas 9:a0fb6c370dbb 156 // robotArm.GetArmMeasure(NM_Load, lastVals);
henryrawas 9:a0fb6c370dbb 157 // DispMeasure("Load", partSize, lastVals);
henryrawas 9:a0fb6c370dbb 158 // measureGroup.SetMeasure(NM_Load, seconds, lastVals);
henryrawas 9:a0fb6c370dbb 159 // MeasureBuf.push(measureGroup);
henryrawas 9:a0fb6c370dbb 160
henryrawas 9:a0fb6c370dbb 161 SendIothubData();
henryrawas 9:a0fb6c370dbb 162
henryrawas 9:a0fb6c370dbb 163 if (!ok)
henryrawas 9:a0fb6c370dbb 164 {
henryrawas 9:a0fb6c370dbb 165 // report HW error
henryrawas 9:a0fb6c370dbb 166 int partix = robotArm.GetLastErrorPart();
henryrawas 9:a0fb6c370dbb 167 int errCode = robotArm.GetLastError();
henryrawas 9:a0fb6c370dbb 168 printf("Hardware error detected part %d, code %d \r\n", partix, errCode);
henryrawas 9:a0fb6c370dbb 169 PushHardwareAlert(AS_Error, partix, errCode);
henryrawas 9:a0fb6c370dbb 170 MainState = MS_Error;
henryrawas 9:a0fb6c370dbb 171 }
henryrawas 9:a0fb6c370dbb 172 }
henryrawas 8:d98e2dec0f40 173
henryrawas 10:9b21566a5ddb 174
henryrawas 10:9b21566a5ddb 175
henryrawas 7:6723f6887d00 176 void ControlArm(const char* cmd)
henryrawas 7:6723f6887d00 177 {
henryrawas 7:6723f6887d00 178 if (strncmp(cmd, "pause", 5) == 0)
henryrawas 7:6723f6887d00 179 {
henryrawas 10:9b21566a5ddb 180 ShowLedBlue();
henryrawas 7:6723f6887d00 181 MainState = MS_Paused;
henryrawas 7:6723f6887d00 182 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 183 }
henryrawas 7:6723f6887d00 184 else if (strncmp(cmd, "resume", 6) == 0)
henryrawas 7:6723f6887d00 185 {
henryrawas 10:9b21566a5ddb 186 ShowLedGreen();
henryrawas 7:6723f6887d00 187 MainState = MS_Running;
henryrawas 7:6723f6887d00 188 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 189 }
henryrawas 10:9b21566a5ddb 190 else if (strncmp(cmd, "cancel", 6) == 0)
henryrawas 10:9b21566a5ddb 191 {
henryrawas 10:9b21566a5ddb 192 ShowLedGreen();
henryrawas 10:9b21566a5ddb 193 MainState = MS_CancelOne;
henryrawas 10:9b21566a5ddb 194 osSignalSet(mainTid, AS_Action);
henryrawas 10:9b21566a5ddb 195 }
henryrawas 7:6723f6887d00 196 else if (strncmp(cmd, "runupdown", 9) == 0)
henryrawas 7:6723f6887d00 197 {
henryrawas 10:9b21566a5ddb 198 ShowLedGreen();
henryrawas 10:9b21566a5ddb 199 MainState = MS_Running;
henryrawas 7:6723f6887d00 200 SequenceQ.put(&UpDownSeq);
henryrawas 7:6723f6887d00 201 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 202 }
henryrawas 7:6723f6887d00 203 else if (strncmp(cmd, "runuptwist", 10) == 0)
henryrawas 7:6723f6887d00 204 {
henryrawas 10:9b21566a5ddb 205 ShowLedGreen();
henryrawas 10:9b21566a5ddb 206 MainState = MS_Running;
henryrawas 7:6723f6887d00 207 SequenceQ.put(&UpTwistSeq);
henryrawas 7:6723f6887d00 208 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 209 }
henryrawas 7:6723f6887d00 210 else if (strncmp(cmd, "runhome", 10) == 0)
henryrawas 7:6723f6887d00 211 {
henryrawas 10:9b21566a5ddb 212 ShowLedGreen();
henryrawas 10:9b21566a5ddb 213 MainState = MS_Running;
henryrawas 7:6723f6887d00 214 SequenceQ.put(&StartSeq);
henryrawas 7:6723f6887d00 215 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 216 }
henryrawas 7:6723f6887d00 217 else if (strncmp(cmd, "runwave", 10) == 0)
henryrawas 7:6723f6887d00 218 {
henryrawas 10:9b21566a5ddb 219 ShowLedGreen();
henryrawas 10:9b21566a5ddb 220 MainState = MS_Running;
henryrawas 7:6723f6887d00 221 SequenceQ.put(&WaveSeq);
henryrawas 7:6723f6887d00 222 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 223 }
henryrawas 8:d98e2dec0f40 224 else if (strncmp(cmd, "runtaps1", 10) == 0)
henryrawas 8:d98e2dec0f40 225 {
henryrawas 10:9b21566a5ddb 226 ShowLedGreen();
henryrawas 10:9b21566a5ddb 227 MainState = MS_Running;
henryrawas 8:d98e2dec0f40 228 SequenceQ.put(&TapsSeq);
henryrawas 8:d98e2dec0f40 229 osSignalSet(mainTid, AS_Action);
henryrawas 8:d98e2dec0f40 230 }
henryrawas 10:9b21566a5ddb 231 else if (strncmp(cmd, "fastwave", 8) == 0)
henryrawas 7:6723f6887d00 232 {
henryrawas 10:9b21566a5ddb 233 ShowLedGreen();
henryrawas 10:9b21566a5ddb 234 MainState = MS_Running;
henryrawas 10:9b21566a5ddb 235 SequenceQ.put(&FastWaveSeq);
henryrawas 7:6723f6887d00 236 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 237 }
henryrawas 7:6723f6887d00 238 }
henryrawas 7:6723f6887d00 239
henryrawas 7:6723f6887d00 240 // run sequence thread timer routine
henryrawas 7:6723f6887d00 241 void NextSeq(void const * tid)
henryrawas 7:6723f6887d00 242 {
henryrawas 7:6723f6887d00 243 RunInDelay = false;
henryrawas 7:6723f6887d00 244 osSignalSet((osThreadId)tid, AS_Action);
henryrawas 7:6723f6887d00 245 }
henryrawas 7:6723f6887d00 246
henryrawas 10:9b21566a5ddb 247 // periodic timer routine
henryrawas 10:9b21566a5ddb 248 void PeriodicStatus(void const * tid)
henryrawas 10:9b21566a5ddb 249 {
henryrawas 10:9b21566a5ddb 250 // make sure we run controller even if idle
henryrawas 10:9b21566a5ddb 251 osSignalSet((osThreadId)tid, AS_Action);
henryrawas 10:9b21566a5ddb 252 }
henryrawas 10:9b21566a5ddb 253
henryrawas 7:6723f6887d00 254 void RunController()
henryrawas 7:6723f6887d00 255 {
henryrawas 7:6723f6887d00 256 // init robot arm
henryrawas 7:6723f6887d00 257 RobotArm robotArm;
henryrawas 7:6723f6887d00 258
henryrawas 7:6723f6887d00 259 int partSize = robotArm.GetNumParts();
henryrawas 7:6723f6887d00 260
henryrawas 10:9b21566a5ddb 261 IdleTimer.start();
henryrawas 10:9b21566a5ddb 262 LastSendMs = (int)IdleTimer.read_ms();
henryrawas 10:9b21566a5ddb 263
henryrawas 7:6723f6887d00 264 // set running state
henryrawas 7:6723f6887d00 265 MainState = MS_Idle;
henryrawas 7:6723f6887d00 266 RunInSequence = false;
henryrawas 7:6723f6887d00 267 RunInMoveStep = false;
henryrawas 7:6723f6887d00 268 RunInDelay = false;
henryrawas 7:6723f6887d00 269
henryrawas 7:6723f6887d00 270 // get initial positions
henryrawas 7:6723f6887d00 271 MeasureGroup measureGroup;
henryrawas 7:6723f6887d00 272 robotArm.GetArmPositions(initPositions);
henryrawas 7:6723f6887d00 273
henryrawas 7:6723f6887d00 274 vector<float> lastVals;
henryrawas 7:6723f6887d00 275
henryrawas 7:6723f6887d00 276 // do inital status report
henryrawas 7:6723f6887d00 277 PushMeasurements(measureGroup, partSize, robotArm);
henryrawas 7:6723f6887d00 278
henryrawas 7:6723f6887d00 279 // Prepare main thread
henryrawas 7:6723f6887d00 280 mainTid = osThreadGetId();
henryrawas 7:6723f6887d00 281 int32_t signals = AS_Action;
henryrawas 7:6723f6887d00 282
henryrawas 7:6723f6887d00 283 // create sequences
henryrawas 7:6723f6887d00 284 MakeSequences(partSize, initPositions);
henryrawas 7:6723f6887d00 285
henryrawas 7:6723f6887d00 286 // add to queue
henryrawas 7:6723f6887d00 287 SequenceQ.put(&StartSeq);
henryrawas 7:6723f6887d00 288
henryrawas 7:6723f6887d00 289 // state for sequence
henryrawas 7:6723f6887d00 290 vector<ActionSequence> *curseq = NULL;
henryrawas 7:6723f6887d00 291 int curseqIx = 0;
henryrawas 10:9b21566a5ddb 292 int loopSeqIx = 0;
henryrawas 10:9b21566a5ddb 293 int loopCounter = 0;
henryrawas 10:9b21566a5ddb 294
henryrawas 7:6723f6887d00 295 // signal to run the default action for demo
henryrawas 7:6723f6887d00 296 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 297
henryrawas 7:6723f6887d00 298 RtosTimer delayTimer(NextSeq, osTimerOnce, (void *)osThreadGetId());
henryrawas 10:9b21566a5ddb 299 RtosTimer statusTimer(PeriodicStatus, osTimerPeriodic, (void *)osThreadGetId());
henryrawas 10:9b21566a5ddb 300 statusTimer.start(IDLESTATUSTO);
henryrawas 7:6723f6887d00 301
henryrawas 7:6723f6887d00 302 MainState = MS_Running;
henryrawas 7:6723f6887d00 303
henryrawas 7:6723f6887d00 304 while( true )
henryrawas 7:6723f6887d00 305 {
henryrawas 7:6723f6887d00 306 osEvent mainEvent = osSignalWait(signals, osWaitForever);
henryrawas 7:6723f6887d00 307
henryrawas 7:6723f6887d00 308 switch (MainState)
henryrawas 7:6723f6887d00 309 {
henryrawas 7:6723f6887d00 310 case MS_Idle:
henryrawas 7:6723f6887d00 311 break;
henryrawas 7:6723f6887d00 312
henryrawas 7:6723f6887d00 313 case MS_Paused:
henryrawas 8:d98e2dec0f40 314 printf("Paused\r\n");
henryrawas 7:6723f6887d00 315 break;
henryrawas 7:6723f6887d00 316
henryrawas 7:6723f6887d00 317 case MS_Stopping:
henryrawas 8:d98e2dec0f40 318 printf("Stopping\r\n");
henryrawas 7:6723f6887d00 319 break;
henryrawas 7:6723f6887d00 320
henryrawas 7:6723f6887d00 321 case MS_Error:
henryrawas 8:d98e2dec0f40 322 printf("Error\r\n");
henryrawas 7:6723f6887d00 323 break;
henryrawas 7:6723f6887d00 324
henryrawas 7:6723f6887d00 325 case MS_CancelOne:
henryrawas 8:d98e2dec0f40 326 printf("Cancel sequence\r\n");
henryrawas 7:6723f6887d00 327 RunInSequence = false;
henryrawas 7:6723f6887d00 328 RunInMoveStep = false;
henryrawas 7:6723f6887d00 329 if (RunInDelay)
henryrawas 7:6723f6887d00 330 {
henryrawas 7:6723f6887d00 331 RunInDelay = false;
henryrawas 7:6723f6887d00 332 delayTimer.stop();
henryrawas 7:6723f6887d00 333 }
henryrawas 7:6723f6887d00 334 MainState = MS_Running;
henryrawas 7:6723f6887d00 335 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 336 break;
henryrawas 7:6723f6887d00 337
henryrawas 7:6723f6887d00 338 case MS_CancelAll:
henryrawas 10:9b21566a5ddb 339 printf("Cancel all\r\n");
henryrawas 7:6723f6887d00 340 RunInSequence = false;
henryrawas 7:6723f6887d00 341 RunInMoveStep = false;
henryrawas 7:6723f6887d00 342 if (RunInDelay)
henryrawas 7:6723f6887d00 343 {
henryrawas 7:6723f6887d00 344 RunInDelay = false;
henryrawas 7:6723f6887d00 345 delayTimer.stop();
henryrawas 7:6723f6887d00 346 }
henryrawas 7:6723f6887d00 347 while (1) {
henryrawas 7:6723f6887d00 348 osEvent evt = SequenceQ.get(0);
henryrawas 7:6723f6887d00 349 if (evt.status != osEventMessage)
henryrawas 7:6723f6887d00 350 break;
henryrawas 7:6723f6887d00 351 }
henryrawas 7:6723f6887d00 352 MainState = MS_Running;
henryrawas 7:6723f6887d00 353 break;
henryrawas 7:6723f6887d00 354
henryrawas 7:6723f6887d00 355 case MS_CancelToM:
henryrawas 10:9b21566a5ddb 356 printf("Cancel until latest\r\n");
henryrawas 7:6723f6887d00 357 RunInSequence = false;
henryrawas 7:6723f6887d00 358 RunInMoveStep = false;
henryrawas 7:6723f6887d00 359 if (RunInDelay)
henryrawas 7:6723f6887d00 360 {
henryrawas 7:6723f6887d00 361 RunInDelay = false;
henryrawas 7:6723f6887d00 362 delayTimer.stop();
henryrawas 7:6723f6887d00 363 }
henryrawas 7:6723f6887d00 364 while (1) {
henryrawas 7:6723f6887d00 365 osEvent evt = SequenceQ.get(0);
henryrawas 7:6723f6887d00 366 if (evt.status != osEventMessage)
henryrawas 7:6723f6887d00 367 break;
henryrawas 7:6723f6887d00 368 else if (evt.value.p == CancelToMatch)
henryrawas 7:6723f6887d00 369 {
henryrawas 7:6723f6887d00 370 curseq = (vector<ActionSequence> *)evt.value.p;
henryrawas 7:6723f6887d00 371 curseqIx = 0;
henryrawas 7:6723f6887d00 372 RunInSequence = true;
henryrawas 7:6723f6887d00 373 }
henryrawas 7:6723f6887d00 374 }
henryrawas 7:6723f6887d00 375 MainState = MS_Running;
henryrawas 7:6723f6887d00 376 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 377 break;
henryrawas 7:6723f6887d00 378
henryrawas 7:6723f6887d00 379 case MS_Running:
henryrawas 7:6723f6887d00 380 if (RunInDelay)
henryrawas 7:6723f6887d00 381 {
henryrawas 7:6723f6887d00 382 // waiting for timer to fire. do nothing
henryrawas 10:9b21566a5ddb 383 printf("Should be delaying. Skip action\r\n");
henryrawas 7:6723f6887d00 384 break;
henryrawas 7:6723f6887d00 385 }
henryrawas 7:6723f6887d00 386 if (!RunInSequence)
henryrawas 7:6723f6887d00 387 {
henryrawas 7:6723f6887d00 388 osEvent evt = SequenceQ.get(0);
henryrawas 7:6723f6887d00 389 if (evt.status == osEventMessage)
henryrawas 7:6723f6887d00 390 {
henryrawas 8:d98e2dec0f40 391 printf("New Seq \r\n");
henryrawas 7:6723f6887d00 392 curseq = (vector<ActionSequence> *)evt.value.p;
henryrawas 7:6723f6887d00 393 curseqIx = 0;
henryrawas 7:6723f6887d00 394 RunInSequence = true;
henryrawas 7:6723f6887d00 395 RunInMoveStep = false;
henryrawas 7:6723f6887d00 396 }
henryrawas 7:6723f6887d00 397 }
henryrawas 7:6723f6887d00 398
henryrawas 7:6723f6887d00 399 if (RunInSequence)
henryrawas 7:6723f6887d00 400 {
henryrawas 7:6723f6887d00 401 if (RunInMoveStep)
henryrawas 7:6723f6887d00 402 {
henryrawas 7:6723f6887d00 403 if (!robotArm.MoveArmPositionTest())
henryrawas 7:6723f6887d00 404 {
henryrawas 7:6723f6887d00 405 // report position error
henryrawas 7:6723f6887d00 406 int ix = robotArm.GetLastErrorPart();
henryrawas 7:6723f6887d00 407 float diff = robotArm.GetLastPosDiff();
henryrawas 8:d98e2dec0f40 408 printf("Position error detected part %d, diff %f \r\n", ix, diff);
henryrawas 7:6723f6887d00 409 PushPositionAlert(AS_Error, ix, diff);
henryrawas 7:6723f6887d00 410 MainState = MS_Error;
henryrawas 7:6723f6887d00 411 break;
henryrawas 7:6723f6887d00 412 }
henryrawas 7:6723f6887d00 413 if (robotArm.MoveArmPositionsHasNext())
henryrawas 7:6723f6887d00 414 {
henryrawas 7:6723f6887d00 415 int delaystep = 0;
henryrawas 7:6723f6887d00 416 bool ok = robotArm.MoveArmPositionsNext();
henryrawas 7:6723f6887d00 417 if (ok)
henryrawas 7:6723f6887d00 418 {
henryrawas 7:6723f6887d00 419 robotArm.MoveArmPositionsDelay(delaystep);
henryrawas 7:6723f6887d00 420 if (delaystep > 0)
henryrawas 7:6723f6887d00 421 {
henryrawas 7:6723f6887d00 422 RunInDelay = true;
henryrawas 7:6723f6887d00 423 delayTimer.start(delaystep);
henryrawas 7:6723f6887d00 424 }
henryrawas 7:6723f6887d00 425 else
henryrawas 7:6723f6887d00 426 {
henryrawas 7:6723f6887d00 427 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 428 }
henryrawas 7:6723f6887d00 429 }
henryrawas 7:6723f6887d00 430 else
henryrawas 7:6723f6887d00 431 {
henryrawas 7:6723f6887d00 432 // report HW error
henryrawas 8:d98e2dec0f40 433 int partix = robotArm.GetLastErrorPart();
henryrawas 8:d98e2dec0f40 434 int errCode = robotArm.GetLastError();
henryrawas 8:d98e2dec0f40 435 printf("Hardware error detected part %d, code %d \r\n", partix, errCode);
henryrawas 8:d98e2dec0f40 436 PushHardwareAlert(AS_Error, partix, errCode);
henryrawas 7:6723f6887d00 437 MainState = MS_Error;
henryrawas 8:d98e2dec0f40 438 break;
henryrawas 7:6723f6887d00 439 }
henryrawas 7:6723f6887d00 440 }
henryrawas 7:6723f6887d00 441 else
henryrawas 7:6723f6887d00 442 {
henryrawas 8:d98e2dec0f40 443 printf("No more Step\r\n");
henryrawas 7:6723f6887d00 444 RunInMoveStep = false;
henryrawas 7:6723f6887d00 445 }
henryrawas 7:6723f6887d00 446 }
henryrawas 7:6723f6887d00 447 if (!RunInMoveStep)
henryrawas 7:6723f6887d00 448 {
henryrawas 8:d98e2dec0f40 449 printf("Next Seq %d\r\n", curseqIx);
henryrawas 7:6723f6887d00 450
henryrawas 7:6723f6887d00 451 if (curseq != NULL)
henryrawas 7:6723f6887d00 452 {
henryrawas 7:6723f6887d00 453 if (curseqIx >= curseq->size())
henryrawas 7:6723f6887d00 454 {
henryrawas 10:9b21566a5ddb 455 printf("sequence completed. Stopping\r\n");
henryrawas 7:6723f6887d00 456 RunInSequence = false;
henryrawas 7:6723f6887d00 457 break;
henryrawas 7:6723f6887d00 458 }
henryrawas 7:6723f6887d00 459
henryrawas 7:6723f6887d00 460 ActionSequence aseq = (*curseq)[curseqIx];
henryrawas 7:6723f6887d00 461 curseqIx++;
henryrawas 7:6723f6887d00 462
henryrawas 7:6723f6887d00 463 switch (aseq.ActionType)
henryrawas 7:6723f6887d00 464 {
henryrawas 7:6723f6887d00 465 case SA_SetGoal:
henryrawas 8:d98e2dec0f40 466 printf(" - Move arm start\r\n");
henryrawas 8:d98e2dec0f40 467 robotArm.MoveArmPositionsStart(aseq.GoalVals, aseq.Param);
henryrawas 8:d98e2dec0f40 468 RunInMoveStep = true;
henryrawas 8:d98e2dec0f40 469 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 470 break;
henryrawas 7:6723f6887d00 471 case SA_Delay:
henryrawas 8:d98e2dec0f40 472 printf(" - Delay\r\n");
henryrawas 7:6723f6887d00 473 RunInDelay = true;
henryrawas 8:d98e2dec0f40 474 delayTimer.start(aseq.Param);
henryrawas 7:6723f6887d00 475 break;
henryrawas 7:6723f6887d00 476 case SA_Status:
henryrawas 8:d98e2dec0f40 477 printf(" - Status\r\n");
henryrawas 8:d98e2dec0f40 478
henryrawas 7:6723f6887d00 479 PushMeasurements(measureGroup, partSize, robotArm);
henryrawas 8:d98e2dec0f40 480
henryrawas 8:d98e2dec0f40 481 robotArm.GetArmLastMeasure(NM_Temperature, lastVals);
henryrawas 8:d98e2dec0f40 482 TestTemperature(lastVals);
henryrawas 8:d98e2dec0f40 483
henryrawas 7:6723f6887d00 484 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 485 break;
henryrawas 10:9b21566a5ddb 486 case SA_LoopBegin:
henryrawas 10:9b21566a5ddb 487 printf(" - LoopBegin\r\n");
henryrawas 10:9b21566a5ddb 488 loopSeqIx = curseqIx;
henryrawas 10:9b21566a5ddb 489 loopCounter = aseq.Param;
henryrawas 7:6723f6887d00 490 osSignalSet(mainTid, AS_Action);
henryrawas 7:6723f6887d00 491 break;
henryrawas 10:9b21566a5ddb 492 case SA_LoopEnd:
henryrawas 10:9b21566a5ddb 493 printf(" - LoopEnd\r\n");
henryrawas 10:9b21566a5ddb 494 loopCounter--;
henryrawas 10:9b21566a5ddb 495 if (loopCounter > 0 && loopSeqIx > 0)
henryrawas 10:9b21566a5ddb 496 curseqIx = loopSeqIx;
henryrawas 10:9b21566a5ddb 497 osSignalSet(mainTid, AS_Action);
henryrawas 10:9b21566a5ddb 498 break;
henryrawas 7:6723f6887d00 499 }
henryrawas 7:6723f6887d00 500 }
henryrawas 7:6723f6887d00 501 }
henryrawas 7:6723f6887d00 502
henryrawas 7:6723f6887d00 503 }
henryrawas 7:6723f6887d00 504 break;
henryrawas 7:6723f6887d00 505 }
henryrawas 7:6723f6887d00 506
henryrawas 10:9b21566a5ddb 507 int now = (int)IdleTimer.read_ms();
henryrawas 10:9b21566a5ddb 508 if (now - LastSendMs > (IDLESTATUSTO - 1000))
henryrawas 10:9b21566a5ddb 509 {
henryrawas 10:9b21566a5ddb 510 PushMeasurements(measureGroup, partSize, robotArm);
henryrawas 10:9b21566a5ddb 511 }
henryrawas 7:6723f6887d00 512 }
henryrawas 7:6723f6887d00 513 }
henryrawas 7:6723f6887d00 514