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@18:224289104fc0, 2016-01-23 (annotated)

Committer:
henryrawas
Date:
Sat Jan 23 00:08:30 2016 +0000
Revision:
18:224289104fc0
Parent:
17:0dbcbd8587fd
Child:
19:2f0ec9ac1238
refactor

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 13:ffeff9b5e513 4 #include "threadapi.h"
henryrawas 7:6723f6887d00 5 #include <vector>
henryrawas 18:224289104fc0 6
henryrawas 18:224289104fc0 7 #include "RobotArm.h"
henryrawas 18:224289104fc0 8 #include "Sequences.h"
henryrawas 18:224289104fc0 9 #include "ControllerIo.h"
henryrawas 18:224289104fc0 10 #include "ControllerUtil.h"
henryrawas 7:6723f6887d00 11
henryrawas 7:6723f6887d00 12 using namespace std;
henryrawas 7:6723f6887d00 13
henryrawas 10:9b21566a5ddb 14
henryrawas 9:a0fb6c370dbb 15 enum MainStates
henryrawas 9:a0fb6c370dbb 16 {
henryrawas 9:a0fb6c370dbb 17 MS_Idle = 0x0,
henryrawas 9:a0fb6c370dbb 18 MS_Running = 0x1,
henryrawas 9:a0fb6c370dbb 19 MS_Paused = 0x2,
henryrawas 13:ffeff9b5e513 20 MS_Error = 0x3,
henryrawas 13:ffeff9b5e513 21 MS_CancelOne = 0x4,
henryrawas 13:ffeff9b5e513 22 MS_CancelAll = 0x5,
henryrawas 13:ffeff9b5e513 23 MS_Resuming = 0x6
henryrawas 9:a0fb6c370dbb 24 };
henryrawas 9:a0fb6c370dbb 25
henryrawas 13:ffeff9b5e513 26 // controller thread runs every 25ms
henryrawas 13:ffeff9b5e513 27 #define CONTROLLER_POLL 25
henryrawas 13:ffeff9b5e513 28
henryrawas 14:570c8071f577 29 // try to send some status this many ms
henryrawas 13:ffeff9b5e513 30 #define SEND_STATUS_TO 500
henryrawas 17:0dbcbd8587fd 31
henryrawas 13:ffeff9b5e513 32
henryrawas 13:ffeff9b5e513 33 // controller polling timer
henryrawas 10:9b21566a5ddb 34 Timer IdleTimer;
henryrawas 13:ffeff9b5e513 35 int NextSendMs = 0;
henryrawas 13:ffeff9b5e513 36 int NextStepMs = 0;
henryrawas 13:ffeff9b5e513 37 int NextPollMs = 0;
henryrawas 10:9b21566a5ddb 38
henryrawas 13:ffeff9b5e513 39 // action sequences
henryrawas 13:ffeff9b5e513 40 Queue<vector<ActionSequence*>, 8> SequenceQ;
henryrawas 13:ffeff9b5e513 41
henryrawas 13:ffeff9b5e513 42 // controller state
henryrawas 11:3a2e6eb9fbb8 43 MainStates MainState;
henryrawas 13:ffeff9b5e513 44
henryrawas 13:ffeff9b5e513 45 bool NeedHwReset;
henryrawas 9:a0fb6c370dbb 46
henryrawas 7:6723f6887d00 47
henryrawas 14:570c8071f577 48 // handle commands received from IoT Hub
henryrawas 14:570c8071f577 49 // Add sequence to queue, change state, etc.
henryrawas 14:570c8071f577 50 void ControlArmCommands(const char* cmd)
henryrawas 7:6723f6887d00 51 {
henryrawas 7:6723f6887d00 52 if (strncmp(cmd, "pause", 5) == 0)
henryrawas 7:6723f6887d00 53 {
henryrawas 10:9b21566a5ddb 54 ShowLedBlue();
henryrawas 7:6723f6887d00 55 MainState = MS_Paused;
henryrawas 7:6723f6887d00 56 }
henryrawas 17:0dbcbd8587fd 57 else if (strncmp(cmd, "alert", 6) == 0)
henryrawas 17:0dbcbd8587fd 58 {
henryrawas 17:0dbcbd8587fd 59 ShowLedRed();
henryrawas 17:0dbcbd8587fd 60 MainState = MS_Error;
henryrawas 17:0dbcbd8587fd 61 }
henryrawas 7:6723f6887d00 62 else if (strncmp(cmd, "resume", 6) == 0)
henryrawas 7:6723f6887d00 63 {
henryrawas 10:9b21566a5ddb 64 ShowLedGreen();
henryrawas 13:ffeff9b5e513 65 MainState = MS_Resuming;
henryrawas 7:6723f6887d00 66 }
henryrawas 10:9b21566a5ddb 67 else if (strncmp(cmd, "cancel", 6) == 0)
henryrawas 10:9b21566a5ddb 68 {
henryrawas 10:9b21566a5ddb 69 ShowLedGreen();
henryrawas 10:9b21566a5ddb 70 MainState = MS_CancelOne;
henryrawas 10:9b21566a5ddb 71 }
henryrawas 7:6723f6887d00 72 else if (strncmp(cmd, "runupdown", 9) == 0)
henryrawas 7:6723f6887d00 73 {
henryrawas 10:9b21566a5ddb 74 ShowLedGreen();
henryrawas 13:ffeff9b5e513 75 if (MainState == MS_Idle || MainState == MS_Paused)
henryrawas 13:ffeff9b5e513 76 MainState = MS_Running;
henryrawas 7:6723f6887d00 77 SequenceQ.put(&UpDownSeq);
henryrawas 7:6723f6887d00 78 }
henryrawas 7:6723f6887d00 79 else if (strncmp(cmd, "runuptwist", 10) == 0)
henryrawas 7:6723f6887d00 80 {
henryrawas 10:9b21566a5ddb 81 ShowLedGreen();
henryrawas 13:ffeff9b5e513 82 if (MainState == MS_Idle || MainState == MS_Paused)
henryrawas 13:ffeff9b5e513 83 MainState = MS_Running;
henryrawas 7:6723f6887d00 84 SequenceQ.put(&UpTwistSeq);
henryrawas 7:6723f6887d00 85 }
henryrawas 7:6723f6887d00 86 else if (strncmp(cmd, "runhome", 10) == 0)
henryrawas 7:6723f6887d00 87 {
henryrawas 10:9b21566a5ddb 88 ShowLedGreen();
henryrawas 13:ffeff9b5e513 89 if (MainState == MS_Idle || MainState == MS_Paused)
henryrawas 13:ffeff9b5e513 90 MainState = MS_Running;
henryrawas 7:6723f6887d00 91 SequenceQ.put(&StartSeq);
henryrawas 7:6723f6887d00 92 }
henryrawas 7:6723f6887d00 93 else if (strncmp(cmd, "runwave", 10) == 0)
henryrawas 7:6723f6887d00 94 {
henryrawas 10:9b21566a5ddb 95 ShowLedGreen();
henryrawas 14:570c8071f577 96 if (MainState == MS_Idle || MainState == MS_Paused)
henryrawas 14:570c8071f577 97 MainState = MS_Running;
henryrawas 7:6723f6887d00 98 SequenceQ.put(&WaveSeq);
henryrawas 7:6723f6887d00 99 }
henryrawas 8:d98e2dec0f40 100 else if (strncmp(cmd, "runtaps1", 10) == 0)
henryrawas 8:d98e2dec0f40 101 {
henryrawas 10:9b21566a5ddb 102 ShowLedGreen();
henryrawas 13:ffeff9b5e513 103 if (MainState == MS_Idle || MainState == MS_Paused)
henryrawas 13:ffeff9b5e513 104 MainState = MS_Running;
henryrawas 8:d98e2dec0f40 105 SequenceQ.put(&TapsSeq);
henryrawas 8:d98e2dec0f40 106 }
henryrawas 10:9b21566a5ddb 107 else if (strncmp(cmd, "fastwave", 8) == 0)
henryrawas 7:6723f6887d00 108 {
henryrawas 10:9b21566a5ddb 109 ShowLedGreen();
henryrawas 14:570c8071f577 110 if (MainState == MS_Idle || MainState == MS_Paused)
henryrawas 14:570c8071f577 111 MainState = MS_Running;
henryrawas 13:ffeff9b5e513 112 SequenceQ.put(&BigWaveSeq);
henryrawas 13:ffeff9b5e513 113 }
henryrawas 13:ffeff9b5e513 114 else if (strncmp(cmd, "beep", 9) == 0)
henryrawas 13:ffeff9b5e513 115 {
henryrawas 13:ffeff9b5e513 116 BuzzerStartMs((int)IdleTimer.read_ms(), 500);
henryrawas 7:6723f6887d00 117 }
henryrawas 7:6723f6887d00 118 }
henryrawas 7:6723f6887d00 119
henryrawas 13:ffeff9b5e513 120
henryrawas 13:ffeff9b5e513 121 // things to initialize early
henryrawas 13:ffeff9b5e513 122 void PrepareController()
henryrawas 7:6723f6887d00 123 {
henryrawas 13:ffeff9b5e513 124 MakeSequences(NUMJOINTS);
henryrawas 7:6723f6887d00 125 }
henryrawas 7:6723f6887d00 126
henryrawas 13:ffeff9b5e513 127 // Run the controller thread loop
henryrawas 7:6723f6887d00 128 void RunController()
henryrawas 7:6723f6887d00 129 {
henryrawas 13:ffeff9b5e513 130 printf("RunController start");
henryrawas 13:ffeff9b5e513 131
henryrawas 7:6723f6887d00 132 // init robot arm
henryrawas 7:6723f6887d00 133 RobotArm robotArm;
henryrawas 13:ffeff9b5e513 134 robotArm.SetStepMs(CONTROLLER_POLL);
henryrawas 7:6723f6887d00 135
henryrawas 7:6723f6887d00 136 int partSize = robotArm.GetNumParts();
henryrawas 7:6723f6887d00 137
henryrawas 10:9b21566a5ddb 138 IdleTimer.start();
henryrawas 10:9b21566a5ddb 139
henryrawas 13:ffeff9b5e513 140 // set running state
henryrawas 7:6723f6887d00 141 MainState = MS_Idle;
henryrawas 7:6723f6887d00 142
henryrawas 13:ffeff9b5e513 143 NeedHwReset = false;
henryrawas 7:6723f6887d00 144
henryrawas 13:ffeff9b5e513 145 // set first sequence to run
henryrawas 7:6723f6887d00 146 SequenceQ.put(&StartSeq);
henryrawas 7:6723f6887d00 147
henryrawas 7:6723f6887d00 148 // state for sequence
henryrawas 13:ffeff9b5e513 149 vector<ActionSequence*> *curseq = NULL;
henryrawas 7:6723f6887d00 150 int curseqIx = 0;
henryrawas 10:9b21566a5ddb 151 int loopSeqIx = 0;
henryrawas 10:9b21566a5ddb 152 int loopCounter = 0;
henryrawas 10:9b21566a5ddb 153
henryrawas 13:ffeff9b5e513 154 // init time outs
henryrawas 13:ffeff9b5e513 155 NextStepMs = 0;
henryrawas 13:ffeff9b5e513 156 NextPollMs = (int)IdleTimer.read_ms();
henryrawas 13:ffeff9b5e513 157 NextSendMs = (int)IdleTimer.read_ms() + SEND_STATUS_TO;
henryrawas 7:6723f6887d00 158
henryrawas 7:6723f6887d00 159 MainState = MS_Running;
henryrawas 7:6723f6887d00 160
henryrawas 7:6723f6887d00 161 while( true )
henryrawas 7:6723f6887d00 162 {
henryrawas 13:ffeff9b5e513 163 int now = (int)IdleTimer.read_ms();
henryrawas 7:6723f6887d00 164
henryrawas 13:ffeff9b5e513 165 // Sleep until next controller poll time
henryrawas 13:ffeff9b5e513 166 if (NextPollMs > now)
henryrawas 13:ffeff9b5e513 167 {
henryrawas 13:ffeff9b5e513 168 ThreadAPI_Sleep(NextPollMs - now);
henryrawas 13:ffeff9b5e513 169 now = (int)IdleTimer.read_ms();
henryrawas 13:ffeff9b5e513 170 }
henryrawas 13:ffeff9b5e513 171 else
henryrawas 11:3a2e6eb9fbb8 172 {
henryrawas 13:ffeff9b5e513 173 printf("too slow %d\r\n", now - NextPollMs);
henryrawas 13:ffeff9b5e513 174 }
henryrawas 13:ffeff9b5e513 175 // set next poll time
henryrawas 13:ffeff9b5e513 176 NextPollMs = now + CONTROLLER_POLL;
henryrawas 13:ffeff9b5e513 177
henryrawas 14:570c8071f577 178 // if had HW error and not in error state, reset error
henryrawas 14:570c8071f577 179 if (NeedHwReset && MainState != MS_Error)
henryrawas 13:ffeff9b5e513 180 {
henryrawas 14:570c8071f577 181 robotArm.ClearErrorState();
henryrawas 14:570c8071f577 182 NeedHwReset = false;
henryrawas 11:3a2e6eb9fbb8 183 }
henryrawas 11:3a2e6eb9fbb8 184
henryrawas 7:6723f6887d00 185 switch (MainState)
henryrawas 7:6723f6887d00 186 {
henryrawas 7:6723f6887d00 187 case MS_Idle:
henryrawas 7:6723f6887d00 188 break;
henryrawas 7:6723f6887d00 189
henryrawas 7:6723f6887d00 190 case MS_Paused:
henryrawas 7:6723f6887d00 191 break;
henryrawas 7:6723f6887d00 192
henryrawas 7:6723f6887d00 193 case MS_Error:
henryrawas 7:6723f6887d00 194 break;
henryrawas 7:6723f6887d00 195
henryrawas 7:6723f6887d00 196 case MS_CancelOne:
henryrawas 14:570c8071f577 197 curseq = NULL;
henryrawas 13:ffeff9b5e513 198 robotArm.MoveArmPositionsEnd();
henryrawas 7:6723f6887d00 199 MainState = MS_Running;
henryrawas 7:6723f6887d00 200 break;
henryrawas 7:6723f6887d00 201
henryrawas 7:6723f6887d00 202 case MS_CancelAll:
henryrawas 14:570c8071f577 203 curseq = NULL;
henryrawas 13:ffeff9b5e513 204 robotArm.MoveArmPositionsEnd();
henryrawas 7:6723f6887d00 205 while (1) {
henryrawas 7:6723f6887d00 206 osEvent evt = SequenceQ.get(0);
henryrawas 7:6723f6887d00 207 if (evt.status != osEventMessage)
henryrawas 7:6723f6887d00 208 break;
henryrawas 7:6723f6887d00 209 }
henryrawas 13:ffeff9b5e513 210 MainState = MS_Running;
henryrawas 13:ffeff9b5e513 211 break;
henryrawas 13:ffeff9b5e513 212
henryrawas 13:ffeff9b5e513 213 case MS_Resuming:
henryrawas 13:ffeff9b5e513 214 robotArm.MoveArmPositionsResume();
henryrawas 7:6723f6887d00 215 MainState = MS_Running;
henryrawas 7:6723f6887d00 216 break;
henryrawas 7:6723f6887d00 217
henryrawas 7:6723f6887d00 218 case MS_Running:
henryrawas 14:570c8071f577 219 if (curseq == NULL)
henryrawas 7:6723f6887d00 220 {
henryrawas 13:ffeff9b5e513 221 // start new sequence of actions
henryrawas 7:6723f6887d00 222 osEvent evt = SequenceQ.get(0);
henryrawas 7:6723f6887d00 223 if (evt.status == osEventMessage)
henryrawas 7:6723f6887d00 224 {
henryrawas 13:ffeff9b5e513 225 printf("New Seq\r\n");
henryrawas 13:ffeff9b5e513 226 curseq = (vector<ActionSequence*> *)evt.value.p;
henryrawas 7:6723f6887d00 227 curseqIx = 0;
henryrawas 7:6723f6887d00 228 }
henryrawas 13:ffeff9b5e513 229 else
henryrawas 13:ffeff9b5e513 230 {
henryrawas 13:ffeff9b5e513 231 MainState = MS_Idle;
henryrawas 13:ffeff9b5e513 232 }
henryrawas 7:6723f6887d00 233 }
henryrawas 7:6723f6887d00 234
henryrawas 14:570c8071f577 235 if (curseq != NULL)
henryrawas 7:6723f6887d00 236 {
henryrawas 14:570c8071f577 237 // check if delaying next move
henryrawas 14:570c8071f577 238 if (NextStepMs > now)
henryrawas 14:570c8071f577 239 break;
henryrawas 14:570c8071f577 240
henryrawas 14:570c8071f577 241 if (robotArm.MoveArmPositionsHasNext())
henryrawas 7:6723f6887d00 242 {
henryrawas 14:570c8071f577 243 bool ok = robotArm.MoveArmPositionsNext();
henryrawas 14:570c8071f577 244 if (!ok)
henryrawas 7:6723f6887d00 245 {
henryrawas 14:570c8071f577 246 // report HW error
henryrawas 14:570c8071f577 247 int partix = robotArm.GetLastErrorPart();
henryrawas 14:570c8071f577 248 int errCode = robotArm.GetLastError();
henryrawas 14:570c8071f577 249 printf("Hardware error detected joint %d, code %d \r\n", partix, errCode);
henryrawas 14:570c8071f577 250 PushHardwareAlert(partix, errCode);
henryrawas 14:570c8071f577 251 NeedHwReset = true;
henryrawas 14:570c8071f577 252 MainState = MS_Error;
henryrawas 14:570c8071f577 253 break;
henryrawas 7:6723f6887d00 254 }
henryrawas 7:6723f6887d00 255 }
henryrawas 14:570c8071f577 256 else
henryrawas 7:6723f6887d00 257 {
henryrawas 14:570c8071f577 258 if (curseqIx >= curseq->size())
henryrawas 7:6723f6887d00 259 {
henryrawas 14:570c8071f577 260 printf("sequence completed. Stopping\r\n");
henryrawas 14:570c8071f577 261 curseq = NULL;
henryrawas 14:570c8071f577 262 break;
henryrawas 14:570c8071f577 263 }
henryrawas 7:6723f6887d00 264
henryrawas 14:570c8071f577 265 ActionSequence* aseq = (*curseq)[curseqIx];
henryrawas 14:570c8071f577 266 curseqIx++;
henryrawas 14:570c8071f577 267
henryrawas 14:570c8071f577 268 switch (aseq->ActionType)
henryrawas 14:570c8071f577 269 {
henryrawas 14:570c8071f577 270 case SA_SetGoal:
henryrawas 14:570c8071f577 271 printf(" - Move arm start\r\n");
henryrawas 14:570c8071f577 272 robotArm.MoveArmPositionsStart(aseq->GoalVals, aseq->Param);
henryrawas 14:570c8071f577 273 break;
henryrawas 14:570c8071f577 274 case SA_Delay:
henryrawas 14:570c8071f577 275 printf(" - Delay\r\n");
henryrawas 14:570c8071f577 276 NextStepMs = aseq->Param + now;
henryrawas 14:570c8071f577 277 break;
henryrawas 14:570c8071f577 278 case SA_LoopBegin:
henryrawas 14:570c8071f577 279 printf(" - LoopBegin\r\n");
henryrawas 14:570c8071f577 280 loopSeqIx = curseqIx;
henryrawas 14:570c8071f577 281 loopCounter = aseq->Param;
henryrawas 14:570c8071f577 282 break;
henryrawas 14:570c8071f577 283 case SA_LoopEnd:
henryrawas 14:570c8071f577 284 printf(" - LoopEnd\r\n");
henryrawas 14:570c8071f577 285 loopCounter--;
henryrawas 14:570c8071f577 286 if (loopCounter > 0 && loopSeqIx > 0)
henryrawas 14:570c8071f577 287 curseqIx = loopSeqIx;
henryrawas 14:570c8071f577 288 break;
henryrawas 7:6723f6887d00 289 }
henryrawas 7:6723f6887d00 290 }
henryrawas 7:6723f6887d00 291
henryrawas 7:6723f6887d00 292 }
henryrawas 7:6723f6887d00 293 break;
henryrawas 7:6723f6887d00 294 }
henryrawas 13:ffeff9b5e513 295
henryrawas 13:ffeff9b5e513 296 bool sendAlert = MainState != MS_Error;
henryrawas 13:ffeff9b5e513 297
henryrawas 13:ffeff9b5e513 298 // get measurements and test thresholds
henryrawas 13:ffeff9b5e513 299 int rc = 0;
henryrawas 15:4bd10f531cdc 300 // call start to clear cached values
henryrawas 15:4bd10f531cdc 301 robotArm.ArmMeasuresTestStart();
henryrawas 15:4bd10f531cdc 302
henryrawas 13:ffeff9b5e513 303 rc = robotArm.ArmMeasuresTest(NM_Load);
henryrawas 14:570c8071f577 304 if (rc == -1 && sendAlert)
henryrawas 13:ffeff9b5e513 305 {
henryrawas 13:ffeff9b5e513 306 PushLoadAlert(robotArm);
henryrawas 13:ffeff9b5e513 307 MainState = MS_Error;
henryrawas 13:ffeff9b5e513 308 }
henryrawas 13:ffeff9b5e513 309
henryrawas 15:4bd10f531cdc 310 rc = robotArm.ArmMeasuresTest(NM_Temperature);
henryrawas 15:4bd10f531cdc 311 if (rc == -1 && sendAlert)
henryrawas 15:4bd10f531cdc 312 {
henryrawas 15:4bd10f531cdc 313 PushTemperatureAlert(robotArm);
henryrawas 15:4bd10f531cdc 314 MainState = MS_Error;
henryrawas 15:4bd10f531cdc 315 }
henryrawas 15:4bd10f531cdc 316
henryrawas 15:4bd10f531cdc 317 rc = robotArm.ArmMeasuresTest(NM_Voltage);
henryrawas 15:4bd10f531cdc 318 if (rc == -1 && sendAlert)
henryrawas 13:ffeff9b5e513 319 {
henryrawas 15:4bd10f531cdc 320 PushVoltageAlert(robotArm);
henryrawas 15:4bd10f531cdc 321 MainState = MS_Error;
henryrawas 15:4bd10f531cdc 322 }
henryrawas 7:6723f6887d00 323
henryrawas 15:4bd10f531cdc 324 rc = robotArm.ArmMeasuresTest(NM_Degrees);
henryrawas 15:4bd10f531cdc 325 if (rc == -1 && sendAlert)
henryrawas 15:4bd10f531cdc 326 {
henryrawas 15:4bd10f531cdc 327 PushPositionAlert(robotArm);
henryrawas 15:4bd10f531cdc 328 MainState = MS_Error;
henryrawas 13:ffeff9b5e513 329 }
henryrawas 15:4bd10f531cdc 330
henryrawas 14:570c8071f577 331 if (rc == -2 && sendAlert)
henryrawas 10:9b21566a5ddb 332 {
henryrawas 13:ffeff9b5e513 333 int partix = robotArm.GetLastErrorPart();
henryrawas 13:ffeff9b5e513 334 int errCode = robotArm.GetLastError();
henryrawas 13:ffeff9b5e513 335 printf("Hardware error detected joint %d, code %d \r\n", partix, errCode);
henryrawas 13:ffeff9b5e513 336 PushHardwareAlert(partix, errCode);
henryrawas 14:570c8071f577 337 NeedHwReset = true;
henryrawas 13:ffeff9b5e513 338 MainState = MS_Error;
henryrawas 13:ffeff9b5e513 339 }
henryrawas 13:ffeff9b5e513 340
henryrawas 13:ffeff9b5e513 341 // check if buzzer needs to be turned off
henryrawas 13:ffeff9b5e513 342 BuzzerPoll(now);
henryrawas 13:ffeff9b5e513 343
henryrawas 13:ffeff9b5e513 344 // check if time to send status
henryrawas 13:ffeff9b5e513 345 if (now >= NextSendMs)
henryrawas 13:ffeff9b5e513 346 {
henryrawas 17:0dbcbd8587fd 347 NextSendMs = now + SEND_STATUS_TO;
henryrawas 17:0dbcbd8587fd 348 if (!PushMeasurements(partSize, robotArm))
henryrawas 13:ffeff9b5e513 349 {
henryrawas 17:0dbcbd8587fd 350 if (sendAlert)
henryrawas 14:570c8071f577 351 {
henryrawas 17:0dbcbd8587fd 352 int partix = robotArm.GetLastErrorPart();
henryrawas 17:0dbcbd8587fd 353 int errCode = robotArm.GetLastError();
henryrawas 17:0dbcbd8587fd 354 printf("Hardware error detected joint %d, code %d \r\n", partix, errCode);
henryrawas 17:0dbcbd8587fd 355 PushHardwareAlert(partix, errCode);
henryrawas 17:0dbcbd8587fd 356 NeedHwReset = true;
henryrawas 17:0dbcbd8587fd 357 MainState = MS_Error;
henryrawas 14:570c8071f577 358 }
henryrawas 13:ffeff9b5e513 359 }
henryrawas 10:9b21566a5ddb 360 }
henryrawas 7:6723f6887d00 361 }
henryrawas 7:6723f6887d00 362 }
henryrawas 7:6723f6887d00 363