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@9:a0fb6c370dbb, 2015-12-31 (annotated)

Committer:
henryrawas
Date:
Thu Dec 31 20:02:58 2015 +0000
Revision:
9:a0fb6c370dbb
Parent:
8:d98e2dec0f40
Child:
10:9b21566a5ddb
report errors

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