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

Committer:
henryrawas
Date:
Thu Dec 31 17:47:55 2015 +0000
Revision:
8:d98e2dec0f40
Parent:
7:6723f6887d00
Child:
9:a0fb6c370dbb
add taps

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