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@17:0dbcbd8587fd, 2016-01-22 (annotated)

Committer:
henryrawas
Date:
Fri Jan 22 01:35:07 2016 +0000
Revision:
17:0dbcbd8587fd
Parent:
15:4bd10f531cdc
Child:
18:224289104fc0
add all measures to circular buffer at once. Handle alert command

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