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@14:570c8071f577, 2016-01-15 (annotated)

Committer:
henryrawas
Date:
Fri Jan 15 23:10:30 2016 +0000
Revision:
14:570c8071f577
Parent:
13:ffeff9b5e513
Child:
15:4bd10f531cdc
cleanups

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 14:570c8071f577 29 // use slower status send rate if paused
henryrawas 13:ffeff9b5e513 30 #define SEND_STATUS_PAUSED_TO 20000
henryrawas 13:ffeff9b5e513 31
henryrawas 13:ffeff9b5e513 32 // controller polling timer
henryrawas 10:9b21566a5ddb 33 Timer IdleTimer;
henryrawas 13:ffeff9b5e513 34 int NextSendMs = 0;
henryrawas 13:ffeff9b5e513 35 int NextStepMs = 0;
henryrawas 13:ffeff9b5e513 36 int NextPollMs = 0;
henryrawas 10:9b21566a5ddb 37
henryrawas 13:ffeff9b5e513 38 // action sequences
henryrawas 13:ffeff9b5e513 39 Queue<vector<ActionSequence*>, 8> SequenceQ;
henryrawas 13:ffeff9b5e513 40
henryrawas 13:ffeff9b5e513 41 // controller state
henryrawas 11:3a2e6eb9fbb8 42 MainStates MainState;
henryrawas 13:ffeff9b5e513 43
henryrawas 13:ffeff9b5e513 44 bool NeedHwReset;
henryrawas 9:a0fb6c370dbb 45
henryrawas 14:570c8071f577 46 extern void PushPositionAlert(RobotArm& robotArm);
henryrawas 14:570c8071f577 47 extern void PushLoadAlert(RobotArm& robotArm);
henryrawas 14:570c8071f577 48 extern void PushTemperatureAlert(RobotArm& robotArm);
henryrawas 14:570c8071f577 49 extern void PushVoltageAlert(RobotArm& robotArm);
henryrawas 14:570c8071f577 50 extern void PushHardwareAlert(int partIx, int code);
henryrawas 14:570c8071f577 51 extern bool PushMeasurements(int partSize, RobotArm& robotArm);
henryrawas 7:6723f6887d00 52
henryrawas 14:570c8071f577 53 // handle commands received from IoT Hub
henryrawas 14:570c8071f577 54 // Add sequence to queue, change state, etc.
henryrawas 14:570c8071f577 55 void ControlArmCommands(const char* cmd)
henryrawas 7:6723f6887d00 56 {
henryrawas 7:6723f6887d00 57 if (strncmp(cmd, "pause", 5) == 0)
henryrawas 7:6723f6887d00 58 {
henryrawas 10:9b21566a5ddb 59 ShowLedBlue();
henryrawas 7:6723f6887d00 60 MainState = MS_Paused;
henryrawas 7:6723f6887d00 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 13:ffeff9b5e513 160 NodeMeasure nextMetric = NM_Temperature;
henryrawas 7:6723f6887d00 161
henryrawas 7:6723f6887d00 162 while( true )
henryrawas 7:6723f6887d00 163 {
henryrawas 13:ffeff9b5e513 164 int now = (int)IdleTimer.read_ms();
henryrawas 7:6723f6887d00 165
henryrawas 13:ffeff9b5e513 166 // Sleep until next controller poll time
henryrawas 13:ffeff9b5e513 167 if (NextPollMs > now)
henryrawas 13:ffeff9b5e513 168 {
henryrawas 13:ffeff9b5e513 169 ThreadAPI_Sleep(NextPollMs - now);
henryrawas 13:ffeff9b5e513 170 now = (int)IdleTimer.read_ms();
henryrawas 13:ffeff9b5e513 171 }
henryrawas 13:ffeff9b5e513 172 else
henryrawas 11:3a2e6eb9fbb8 173 {
henryrawas 13:ffeff9b5e513 174 printf("too slow %d\r\n", now - NextPollMs);
henryrawas 13:ffeff9b5e513 175 }
henryrawas 13:ffeff9b5e513 176 // set next poll time
henryrawas 13:ffeff9b5e513 177 NextPollMs = now + CONTROLLER_POLL;
henryrawas 13:ffeff9b5e513 178
henryrawas 14:570c8071f577 179 // if had HW error and not in error state, reset error
henryrawas 14:570c8071f577 180 if (NeedHwReset && MainState != MS_Error)
henryrawas 13:ffeff9b5e513 181 {
henryrawas 14:570c8071f577 182 robotArm.ClearErrorState();
henryrawas 14:570c8071f577 183 NeedHwReset = false;
henryrawas 11:3a2e6eb9fbb8 184 }
henryrawas 11:3a2e6eb9fbb8 185
henryrawas 7:6723f6887d00 186 switch (MainState)
henryrawas 7:6723f6887d00 187 {
henryrawas 7:6723f6887d00 188 case MS_Idle:
henryrawas 7:6723f6887d00 189 break;
henryrawas 7:6723f6887d00 190
henryrawas 7:6723f6887d00 191 case MS_Paused:
henryrawas 7:6723f6887d00 192 break;
henryrawas 7:6723f6887d00 193
henryrawas 7:6723f6887d00 194 case MS_Error:
henryrawas 7:6723f6887d00 195 break;
henryrawas 7:6723f6887d00 196
henryrawas 7:6723f6887d00 197 case MS_CancelOne:
henryrawas 14:570c8071f577 198 curseq = NULL;
henryrawas 13:ffeff9b5e513 199 robotArm.MoveArmPositionsEnd();
henryrawas 7:6723f6887d00 200 MainState = MS_Running;
henryrawas 13:ffeff9b5e513 201 // avoid next poll delay
henryrawas 13:ffeff9b5e513 202 NextPollMs = now;
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 // avoid next poll delay
henryrawas 13:ffeff9b5e513 214 NextPollMs = now;
henryrawas 13:ffeff9b5e513 215 MainState = MS_Running;
henryrawas 13:ffeff9b5e513 216 break;
henryrawas 13:ffeff9b5e513 217
henryrawas 13:ffeff9b5e513 218 case MS_Resuming:
henryrawas 13:ffeff9b5e513 219 robotArm.MoveArmPositionsResume();
henryrawas 13:ffeff9b5e513 220 // avoid next poll delay
henryrawas 13:ffeff9b5e513 221 NextPollMs = now;
henryrawas 7:6723f6887d00 222 MainState = MS_Running;
henryrawas 7:6723f6887d00 223 break;
henryrawas 7:6723f6887d00 224
henryrawas 7:6723f6887d00 225 case MS_Running:
henryrawas 14:570c8071f577 226 if (curseq == NULL)
henryrawas 7:6723f6887d00 227 {
henryrawas 13:ffeff9b5e513 228 // start new sequence of actions
henryrawas 7:6723f6887d00 229 osEvent evt = SequenceQ.get(0);
henryrawas 7:6723f6887d00 230 if (evt.status == osEventMessage)
henryrawas 7:6723f6887d00 231 {
henryrawas 13:ffeff9b5e513 232 printf("New Seq\r\n");
henryrawas 13:ffeff9b5e513 233 curseq = (vector<ActionSequence*> *)evt.value.p;
henryrawas 7:6723f6887d00 234 curseqIx = 0;
henryrawas 7:6723f6887d00 235 }
henryrawas 13:ffeff9b5e513 236 else
henryrawas 13:ffeff9b5e513 237 {
henryrawas 13:ffeff9b5e513 238 MainState = MS_Idle;
henryrawas 13:ffeff9b5e513 239 }
henryrawas 7:6723f6887d00 240 }
henryrawas 7:6723f6887d00 241
henryrawas 14:570c8071f577 242 if (curseq != NULL)
henryrawas 7:6723f6887d00 243 {
henryrawas 14:570c8071f577 244 // check if delaying next move
henryrawas 14:570c8071f577 245 if (NextStepMs > now)
henryrawas 14:570c8071f577 246 break;
henryrawas 14:570c8071f577 247
henryrawas 14:570c8071f577 248 if (robotArm.MoveArmPositionsHasNext())
henryrawas 7:6723f6887d00 249 {
henryrawas 14:570c8071f577 250 bool ok = robotArm.MoveArmPositionsNext();
henryrawas 14:570c8071f577 251 if (!ok)
henryrawas 7:6723f6887d00 252 {
henryrawas 14:570c8071f577 253 // report HW error
henryrawas 14:570c8071f577 254 int partix = robotArm.GetLastErrorPart();
henryrawas 14:570c8071f577 255 int errCode = robotArm.GetLastError();
henryrawas 14:570c8071f577 256 printf("Hardware error detected joint %d, code %d \r\n", partix, errCode);
henryrawas 14:570c8071f577 257 PushHardwareAlert(partix, errCode);
henryrawas 14:570c8071f577 258 NeedHwReset = true;
henryrawas 14:570c8071f577 259 MainState = MS_Error;
henryrawas 14:570c8071f577 260 break;
henryrawas 7:6723f6887d00 261 }
henryrawas 7:6723f6887d00 262 }
henryrawas 14:570c8071f577 263 else
henryrawas 7:6723f6887d00 264 {
henryrawas 14:570c8071f577 265 if (curseqIx >= curseq->size())
henryrawas 7:6723f6887d00 266 {
henryrawas 14:570c8071f577 267 printf("sequence completed. Stopping\r\n");
henryrawas 14:570c8071f577 268 curseq = NULL;
henryrawas 14:570c8071f577 269 break;
henryrawas 14:570c8071f577 270 }
henryrawas 7:6723f6887d00 271
henryrawas 14:570c8071f577 272 ActionSequence* aseq = (*curseq)[curseqIx];
henryrawas 14:570c8071f577 273 curseqIx++;
henryrawas 14:570c8071f577 274
henryrawas 14:570c8071f577 275 switch (aseq->ActionType)
henryrawas 14:570c8071f577 276 {
henryrawas 14:570c8071f577 277 case SA_SetGoal:
henryrawas 14:570c8071f577 278 printf(" - Move arm start\r\n");
henryrawas 14:570c8071f577 279 robotArm.MoveArmPositionsStart(aseq->GoalVals, aseq->Param);
henryrawas 14:570c8071f577 280 break;
henryrawas 14:570c8071f577 281 case SA_Delay:
henryrawas 14:570c8071f577 282 printf(" - Delay\r\n");
henryrawas 14:570c8071f577 283 NextStepMs = aseq->Param + now;
henryrawas 14:570c8071f577 284 break;
henryrawas 14:570c8071f577 285 case SA_LoopBegin:
henryrawas 14:570c8071f577 286 printf(" - LoopBegin\r\n");
henryrawas 14:570c8071f577 287 loopSeqIx = curseqIx;
henryrawas 14:570c8071f577 288 loopCounter = aseq->Param;
henryrawas 14:570c8071f577 289 break;
henryrawas 14:570c8071f577 290 case SA_LoopEnd:
henryrawas 14:570c8071f577 291 printf(" - LoopEnd\r\n");
henryrawas 14:570c8071f577 292 loopCounter--;
henryrawas 14:570c8071f577 293 if (loopCounter > 0 && loopSeqIx > 0)
henryrawas 14:570c8071f577 294 curseqIx = loopSeqIx;
henryrawas 14:570c8071f577 295 break;
henryrawas 7:6723f6887d00 296 }
henryrawas 7:6723f6887d00 297 }
henryrawas 7:6723f6887d00 298
henryrawas 7:6723f6887d00 299 }
henryrawas 7:6723f6887d00 300 break;
henryrawas 7:6723f6887d00 301 }
henryrawas 13:ffeff9b5e513 302
henryrawas 13:ffeff9b5e513 303 bool sendAlert = MainState != MS_Error;
henryrawas 13:ffeff9b5e513 304
henryrawas 13:ffeff9b5e513 305 // get measurements and test thresholds
henryrawas 13:ffeff9b5e513 306 // Reading all values takes too long,
henryrawas 13:ffeff9b5e513 307 // so we read the load and 1 other value each time
henryrawas 13:ffeff9b5e513 308 int rc = 0;
henryrawas 13:ffeff9b5e513 309 rc = robotArm.ArmMeasuresTest(NM_Load);
henryrawas 14:570c8071f577 310 if (rc == -1 && sendAlert)
henryrawas 13:ffeff9b5e513 311 {
henryrawas 13:ffeff9b5e513 312 PushLoadAlert(robotArm);
henryrawas 13:ffeff9b5e513 313 MainState = MS_Error;
henryrawas 13:ffeff9b5e513 314 }
henryrawas 13:ffeff9b5e513 315
henryrawas 13:ffeff9b5e513 316 switch (nextMetric)
henryrawas 13:ffeff9b5e513 317 {
henryrawas 13:ffeff9b5e513 318 case NM_Temperature:
henryrawas 13:ffeff9b5e513 319 rc = robotArm.ArmMeasuresTest(NM_Temperature);
henryrawas 14:570c8071f577 320 if (rc == -1 && sendAlert)
henryrawas 13:ffeff9b5e513 321 {
henryrawas 13:ffeff9b5e513 322 PushTemperatureAlert(robotArm);
henryrawas 13:ffeff9b5e513 323 MainState = MS_Error;
henryrawas 13:ffeff9b5e513 324 }
henryrawas 13:ffeff9b5e513 325 nextMetric = NM_Voltage;
henryrawas 13:ffeff9b5e513 326 break;
henryrawas 13:ffeff9b5e513 327
henryrawas 13:ffeff9b5e513 328 case NM_Voltage:
henryrawas 13:ffeff9b5e513 329 rc = robotArm.ArmMeasuresTest(NM_Voltage);
henryrawas 14:570c8071f577 330 if (rc == -1 && sendAlert)
henryrawas 13:ffeff9b5e513 331 {
henryrawas 13:ffeff9b5e513 332 PushVoltageAlert(robotArm);
henryrawas 13:ffeff9b5e513 333 MainState = MS_Error;
henryrawas 13:ffeff9b5e513 334 }
henryrawas 13:ffeff9b5e513 335 nextMetric = NM_Degrees;
henryrawas 13:ffeff9b5e513 336 break;
henryrawas 13:ffeff9b5e513 337
henryrawas 13:ffeff9b5e513 338
henryrawas 13:ffeff9b5e513 339 case NM_Degrees:
henryrawas 13:ffeff9b5e513 340 rc = robotArm.ArmMeasuresTest(NM_Degrees);
henryrawas 14:570c8071f577 341 if (rc == -1 && sendAlert)
henryrawas 13:ffeff9b5e513 342 {
henryrawas 13:ffeff9b5e513 343 PushPositionAlert(robotArm);
henryrawas 13:ffeff9b5e513 344 MainState = MS_Error;
henryrawas 13:ffeff9b5e513 345 }
henryrawas 13:ffeff9b5e513 346 nextMetric = NM_Temperature;
henryrawas 13:ffeff9b5e513 347 break;
henryrawas 7:6723f6887d00 348
henryrawas 13:ffeff9b5e513 349 default:
henryrawas 13:ffeff9b5e513 350 nextMetric = NM_Temperature;
henryrawas 13:ffeff9b5e513 351 break;
henryrawas 13:ffeff9b5e513 352 }
henryrawas 14:570c8071f577 353 if (rc == -2 && sendAlert)
henryrawas 10:9b21566a5ddb 354 {
henryrawas 13:ffeff9b5e513 355 int partix = robotArm.GetLastErrorPart();
henryrawas 13:ffeff9b5e513 356 int errCode = robotArm.GetLastError();
henryrawas 13:ffeff9b5e513 357 printf("Hardware error detected joint %d, code %d \r\n", partix, errCode);
henryrawas 13:ffeff9b5e513 358 PushHardwareAlert(partix, errCode);
henryrawas 14:570c8071f577 359 NeedHwReset = true;
henryrawas 13:ffeff9b5e513 360 MainState = MS_Error;
henryrawas 13:ffeff9b5e513 361 }
henryrawas 13:ffeff9b5e513 362
henryrawas 13:ffeff9b5e513 363 // check if buzzer needs to be turned off
henryrawas 13:ffeff9b5e513 364 BuzzerPoll(now);
henryrawas 13:ffeff9b5e513 365
henryrawas 13:ffeff9b5e513 366 // check if time to send status
henryrawas 13:ffeff9b5e513 367 if (now >= NextSendMs)
henryrawas 13:ffeff9b5e513 368 {
henryrawas 13:ffeff9b5e513 369 // if paused, use longer time out for sending data
henryrawas 13:ffeff9b5e513 370 if (MainState != MS_Paused ||
henryrawas 13:ffeff9b5e513 371 now > NextSendMs + SEND_STATUS_PAUSED_TO)
henryrawas 13:ffeff9b5e513 372 {
henryrawas 13:ffeff9b5e513 373 NextSendMs = now + SEND_STATUS_TO;
henryrawas 14:570c8071f577 374 if (!PushMeasurements(partSize, robotArm))
henryrawas 14:570c8071f577 375 {
henryrawas 14:570c8071f577 376 if (sendAlert)
henryrawas 14:570c8071f577 377 {
henryrawas 14:570c8071f577 378 int partix = robotArm.GetLastErrorPart();
henryrawas 14:570c8071f577 379 int errCode = robotArm.GetLastError();
henryrawas 14:570c8071f577 380 printf("Hardware error detected joint %d, code %d \r\n", partix, errCode);
henryrawas 14:570c8071f577 381 PushHardwareAlert(partix, errCode);
henryrawas 14:570c8071f577 382 NeedHwReset = true;
henryrawas 14:570c8071f577 383 MainState = MS_Error;
henryrawas 14:570c8071f577 384 }
henryrawas 14:570c8071f577 385 }
henryrawas 13:ffeff9b5e513 386 }
henryrawas 10:9b21566a5ddb 387 }
henryrawas 7:6723f6887d00 388 }
henryrawas 7:6723f6887d00 389 }
henryrawas 7:6723f6887d00 390