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@11:3a2e6eb9fbb8, 2016-01-06 (annotated)

Committer:
henryrawas
Date:
Wed Jan 06 22:25:51 2016 +0000
Revision:
11:3a2e6eb9fbb8
Parent:
10:9b21566a5ddb
Child:
13:ffeff9b5e513
error reset. Better sequences;

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