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@33:8b9dcbf6d8ec, 2016-02-04 (annotated)

Committer:
henryrawas
Date:
Thu Feb 04 20:10:33 2016 +0000
Revision:
33:8b9dcbf6d8ec
Parent:
19:2f0ec9ac1238 
update libs

Who changed what in which revision?

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