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@13:ffeff9b5e513, 2016-01-15 (annotated)

Committer:
henryrawas
Date:
Fri Jan 15 22:02:46 2016 +0000
Revision:
13:ffeff9b5e513
Parent:
11:3a2e6eb9fbb8
Child:
14:570c8071f577
Always test status and send data

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 <NTPClient.h>
henryrawas 7:6723f6887d00 6 #include <DynamixelBus.h>
henryrawas 7:6723f6887d00 7 #include <AX12.h>
henryrawas 7:6723f6887d00 8 #include <Terminal.h>
henryrawas 7:6723f6887d00 9 #include <vector>
henryrawas 7:6723f6887d00 10 #include <RobotArm.h>
henryrawas 7:6723f6887d00 11 #include <MeasureBuf.h>
henryrawas 7:6723f6887d00 12 #include <IothubRobotArm.h>
henryrawas 7:6723f6887d00 13 #include <ActionBuf.h>
henryrawas 7:6723f6887d00 14 #include <Sequences.h>
henryrawas 10:9b21566a5ddb 15 #include <ControllerIo.h>
henryrawas 7:6723f6887d00 16
henryrawas 7:6723f6887d00 17 using namespace std;
henryrawas 7:6723f6887d00 18
henryrawas 10:9b21566a5ddb 19
henryrawas 9:a0fb6c370dbb 20 enum MainStates
henryrawas 9:a0fb6c370dbb 21 {
henryrawas 9:a0fb6c370dbb 22 MS_Idle = 0x0,
henryrawas 9:a0fb6c370dbb 23 MS_Running = 0x1,
henryrawas 9:a0fb6c370dbb 24 MS_Paused = 0x2,
henryrawas 13:ffeff9b5e513 25 MS_Error = 0x3,
henryrawas 13:ffeff9b5e513 26 MS_CancelOne = 0x4,
henryrawas 13:ffeff9b5e513 27 MS_CancelAll = 0x5,
henryrawas 13:ffeff9b5e513 28 MS_Resuming = 0x6
henryrawas 9:a0fb6c370dbb 29 };
henryrawas 9:a0fb6c370dbb 30
henryrawas 13:ffeff9b5e513 31 // controller thread runs every 25ms
henryrawas 13:ffeff9b5e513 32 #define CONTROLLER_POLL 25
henryrawas 13:ffeff9b5e513 33
henryrawas 13:ffeff9b5e513 34 // try to send some status every few seconds
henryrawas 13:ffeff9b5e513 35 #define SEND_STATUS_TO 500
henryrawas 13:ffeff9b5e513 36 // use slower send rate if paused
henryrawas 13:ffeff9b5e513 37 #define SEND_STATUS_PAUSED_TO 20000
henryrawas 13:ffeff9b5e513 38
henryrawas 13:ffeff9b5e513 39 // controller polling timer
henryrawas 10:9b21566a5ddb 40 Timer IdleTimer;
henryrawas 13:ffeff9b5e513 41 int NextSendMs = 0;
henryrawas 13:ffeff9b5e513 42 int NextStepMs = 0;
henryrawas 13:ffeff9b5e513 43 int NextPollMs = 0;
henryrawas 10:9b21566a5ddb 44
henryrawas 13:ffeff9b5e513 45 // action sequences
henryrawas 13:ffeff9b5e513 46 Queue<vector<ActionSequence*>, 8> SequenceQ;
henryrawas 13:ffeff9b5e513 47
henryrawas 13:ffeff9b5e513 48 // controller state
henryrawas 11:3a2e6eb9fbb8 49 MainStates MainState;
henryrawas 9:a0fb6c370dbb 50 bool RunInSequence;
henryrawas 9:a0fb6c370dbb 51 bool RunInMoveStep;
henryrawas 13:ffeff9b5e513 52
henryrawas 13:ffeff9b5e513 53 bool NeedHwReset;
henryrawas 9:a0fb6c370dbb 54
henryrawas 13:ffeff9b5e513 55 // utility method
henryrawas 13:ffeff9b5e513 56 void DispMeasure(char* label, int partSize, float vals[])
henryrawas 8:d98e2dec0f40 57 {
henryrawas 13:ffeff9b5e513 58 // printf("%s: ", label);
henryrawas 13:ffeff9b5e513 59 // for (int ix = 0; ix < partSize; ix++)
henryrawas 13:ffeff9b5e513 60 // {
henryrawas 13:ffeff9b5e513 61 // printf("%d:%f ", ix, vals[ix]);
henryrawas 13:ffeff9b5e513 62 // }
henryrawas 13:ffeff9b5e513 63 // printf("\r\n");
henryrawas 8:d98e2dec0f40 64 }
henryrawas 7:6723f6887d00 65
henryrawas 13:ffeff9b5e513 66 // send alert message
henryrawas 13:ffeff9b5e513 67 void PushAlert(char* msg, char* atype)
henryrawas 7:6723f6887d00 68 {
henryrawas 7:6723f6887d00 69 Alert alert;
henryrawas 7:6723f6887d00 70 time_t seconds = time(NULL);
henryrawas 7:6723f6887d00 71
henryrawas 13:ffeff9b5e513 72 alert.SetAlert(seconds, msg, atype);
henryrawas 7:6723f6887d00 73 AlertBuf.push(alert);
henryrawas 7:6723f6887d00 74 }
henryrawas 7:6723f6887d00 75
henryrawas 13:ffeff9b5e513 76 // send position alert
henryrawas 13:ffeff9b5e513 77 void PushPositionAlert(RobotArm& robotArm)
henryrawas 7:6723f6887d00 78 {
henryrawas 13:ffeff9b5e513 79 // stop trying to move
henryrawas 13:ffeff9b5e513 80 float diff = robotArm.GetLastPosDiff();
henryrawas 13:ffeff9b5e513 81 int ix = robotArm.GetLastErrorPart();
henryrawas 13:ffeff9b5e513 82 robotArm.MoveArmPositionsStop();
henryrawas 13:ffeff9b5e513 83
henryrawas 13:ffeff9b5e513 84 // report load error
henryrawas 13:ffeff9b5e513 85 printf("Position error detected joint %d, value diff %f\r\n", ix, diff);
henryrawas 13:ffeff9b5e513 86
henryrawas 7:6723f6887d00 87 Alert alert;
henryrawas 7:6723f6887d00 88 time_t seconds = time(NULL);
henryrawas 7:6723f6887d00 89
henryrawas 10:9b21566a5ddb 90 ShowLedRed();
henryrawas 13:ffeff9b5e513 91 alert.SetPositionAlert(seconds, ix, diff);
henryrawas 7:6723f6887d00 92 AlertBuf.push(alert);
henryrawas 13:ffeff9b5e513 93
henryrawas 13:ffeff9b5e513 94 BuzzerStartMs((int)IdleTimer.read_ms(), 500);
henryrawas 7:6723f6887d00 95 }
henryrawas 7:6723f6887d00 96
henryrawas 13:ffeff9b5e513 97 // send load alert
henryrawas 13:ffeff9b5e513 98 void PushLoadAlert(RobotArm& robotArm)
henryrawas 13:ffeff9b5e513 99 {
henryrawas 13:ffeff9b5e513 100 float lastVals[NUMJOINTS];
henryrawas 13:ffeff9b5e513 101
henryrawas 13:ffeff9b5e513 102 // stop trying to move
henryrawas 13:ffeff9b5e513 103 robotArm.GetArmLastMeasure(NM_Load, lastVals);
henryrawas 13:ffeff9b5e513 104 int ix = robotArm.GetLastErrorPart();
henryrawas 13:ffeff9b5e513 105 robotArm.MoveArmPositionsStop();
henryrawas 13:ffeff9b5e513 106
henryrawas 13:ffeff9b5e513 107 // report load error
henryrawas 13:ffeff9b5e513 108 printf("Load error detected joint %d, value %f\r\n", ix, lastVals[ix]);
henryrawas 13:ffeff9b5e513 109
henryrawas 13:ffeff9b5e513 110 Alert alert;
henryrawas 13:ffeff9b5e513 111 time_t seconds = time(NULL);
henryrawas 13:ffeff9b5e513 112
henryrawas 13:ffeff9b5e513 113 ShowLedRed();
henryrawas 13:ffeff9b5e513 114 alert.SetLoadAlert(seconds, ix, lastVals[ix]);
henryrawas 13:ffeff9b5e513 115 AlertBuf.push(alert);
henryrawas 13:ffeff9b5e513 116
henryrawas 13:ffeff9b5e513 117 BuzzerStartMs((int)IdleTimer.read_ms(), 500);
henryrawas 13:ffeff9b5e513 118 }
henryrawas 13:ffeff9b5e513 119
henryrawas 13:ffeff9b5e513 120 // send hardware error alert
henryrawas 13:ffeff9b5e513 121 void PushHardwareAlert(int partIx, int code)
henryrawas 8:d98e2dec0f40 122 {
henryrawas 8:d98e2dec0f40 123 Alert alert;
henryrawas 8:d98e2dec0f40 124 time_t seconds = time(NULL);
henryrawas 8:d98e2dec0f40 125
henryrawas 10:9b21566a5ddb 126 ShowLedRed();
henryrawas 13:ffeff9b5e513 127 alert.SetHardwareAlert(seconds, partIx, code);
henryrawas 8:d98e2dec0f40 128 AlertBuf.push(alert);
henryrawas 13:ffeff9b5e513 129 NeedHwReset = true;
henryrawas 13:ffeff9b5e513 130
henryrawas 13:ffeff9b5e513 131 BuzzerStartMs((int)IdleTimer.read_ms(), 500);
henryrawas 8:d98e2dec0f40 132 }
henryrawas 7:6723f6887d00 133
henryrawas 13:ffeff9b5e513 134 // send temperature alert
henryrawas 13:ffeff9b5e513 135 void PushTemperatureAlert(RobotArm& robotArm)
henryrawas 8:d98e2dec0f40 136 {
henryrawas 13:ffeff9b5e513 137 float lastVals[NUMJOINTS];
henryrawas 13:ffeff9b5e513 138
henryrawas 13:ffeff9b5e513 139 // stop trying to move
henryrawas 13:ffeff9b5e513 140 robotArm.GetArmLastMeasure(NM_Temperature, lastVals);
henryrawas 13:ffeff9b5e513 141 int ix = robotArm.GetLastErrorPart();
henryrawas 13:ffeff9b5e513 142 robotArm.MoveArmPositionsStop();
henryrawas 13:ffeff9b5e513 143
henryrawas 13:ffeff9b5e513 144 // report load error
henryrawas 13:ffeff9b5e513 145 printf("Temperature error detected joint %d, value %f\r\n", ix, lastVals[ix]);
henryrawas 13:ffeff9b5e513 146
henryrawas 8:d98e2dec0f40 147 Alert alert;
henryrawas 8:d98e2dec0f40 148 time_t seconds = time(NULL);
henryrawas 8:d98e2dec0f40 149
henryrawas 10:9b21566a5ddb 150 ShowLedRed();
henryrawas 13:ffeff9b5e513 151 alert.SetTemperatureAlert(seconds, ix, lastVals[ix]);
henryrawas 8:d98e2dec0f40 152 AlertBuf.push(alert);
henryrawas 13:ffeff9b5e513 153
henryrawas 13:ffeff9b5e513 154 BuzzerStartMs((int)IdleTimer.read_ms(), 500);
henryrawas 8:d98e2dec0f40 155 }
henryrawas 7:6723f6887d00 156
henryrawas 13:ffeff9b5e513 157 // send temperature alert
henryrawas 13:ffeff9b5e513 158 void PushVoltageAlert(RobotArm& robotArm)
henryrawas 8:d98e2dec0f40 159 {
henryrawas 13:ffeff9b5e513 160 float lastVals[NUMJOINTS];
henryrawas 13:ffeff9b5e513 161
henryrawas 13:ffeff9b5e513 162 // stop trying to move
henryrawas 13:ffeff9b5e513 163 robotArm.GetArmLastMeasure(NM_Voltage, lastVals);
henryrawas 13:ffeff9b5e513 164 int ix = robotArm.GetLastErrorPart();
henryrawas 13:ffeff9b5e513 165 robotArm.MoveArmPositionsStop();
henryrawas 13:ffeff9b5e513 166
henryrawas 13:ffeff9b5e513 167 // report load error
henryrawas 13:ffeff9b5e513 168 printf("Voltage error detected joint %d, value %f\r\n", ix, lastVals[ix]);
henryrawas 8:d98e2dec0f40 169
henryrawas 13:ffeff9b5e513 170 Alert alert;
henryrawas 13:ffeff9b5e513 171 time_t seconds = time(NULL);
henryrawas 13:ffeff9b5e513 172
henryrawas 13:ffeff9b5e513 173 ShowLedRed();
henryrawas 13:ffeff9b5e513 174 alert.SetVoltageAlert(seconds, ix, lastVals[ix]);
henryrawas 13:ffeff9b5e513 175 AlertBuf.push(alert);
henryrawas 13:ffeff9b5e513 176
henryrawas 13:ffeff9b5e513 177 BuzzerStartMs((int)IdleTimer.read_ms(), 500);
henryrawas 8:d98e2dec0f40 178 }
henryrawas 8:d98e2dec0f40 179
henryrawas 10:9b21566a5ddb 180
henryrawas 13:ffeff9b5e513 181 void PushMeasurements(int partSize, RobotArm& robotArm)
henryrawas 9:a0fb6c370dbb 182 {
henryrawas 13:ffeff9b5e513 183 // space for getting measurements from arm
henryrawas 13:ffeff9b5e513 184 MeasureGroup measureGroup;
henryrawas 10:9b21566a5ddb 185
henryrawas 13:ffeff9b5e513 186 float lastVals[NUMJOINTS];
henryrawas 13:ffeff9b5e513 187 time_t seconds = time(NULL);
henryrawas 13:ffeff9b5e513 188
henryrawas 9:a0fb6c370dbb 189 bool ok = true;
henryrawas 13:ffeff9b5e513 190
henryrawas 13:ffeff9b5e513 191 ok = robotArm.GetArmLastMeasure(NM_Temperature, lastVals);
henryrawas 9:a0fb6c370dbb 192 DispMeasure("Temperatures", partSize, lastVals);
henryrawas 13:ffeff9b5e513 193 measureGroup.SetMeasure(NM_Temperature, seconds, partSize, lastVals);
henryrawas 9:a0fb6c370dbb 194 MeasureBuf.push(measureGroup);
henryrawas 9:a0fb6c370dbb 195
henryrawas 9:a0fb6c370dbb 196 if (ok)
henryrawas 9:a0fb6c370dbb 197 {
henryrawas 13:ffeff9b5e513 198 ok = robotArm.GetArmLastMeasure(NM_Voltage, lastVals);
henryrawas 9:a0fb6c370dbb 199 DispMeasure("Voltage", partSize, lastVals);
henryrawas 13:ffeff9b5e513 200 measureGroup.SetMeasure(NM_Voltage, seconds, partSize, lastVals);
henryrawas 9:a0fb6c370dbb 201 MeasureBuf.push(measureGroup);
henryrawas 9:a0fb6c370dbb 202 }
henryrawas 9:a0fb6c370dbb 203
henryrawas 9:a0fb6c370dbb 204 if (ok)
henryrawas 9:a0fb6c370dbb 205 {
henryrawas 13:ffeff9b5e513 206 ok = robotArm.GetArmLastMeasure(NM_Degrees, lastVals);
henryrawas 9:a0fb6c370dbb 207 DispMeasure("Rotation", partSize, lastVals);
henryrawas 13:ffeff9b5e513 208 measureGroup.SetMeasure(NM_Degrees, seconds, partSize, lastVals);
henryrawas 9:a0fb6c370dbb 209 MeasureBuf.push(measureGroup);
henryrawas 9:a0fb6c370dbb 210 }
henryrawas 9:a0fb6c370dbb 211
henryrawas 13:ffeff9b5e513 212 if (ok)
henryrawas 13:ffeff9b5e513 213 {
henryrawas 13:ffeff9b5e513 214 robotArm.GetArmLastMeasure(NM_Load, lastVals);
henryrawas 13:ffeff9b5e513 215 DispMeasure("Load", partSize, lastVals);
henryrawas 13:ffeff9b5e513 216 measureGroup.SetMeasure(NM_Load, seconds, partSize, lastVals);
henryrawas 13:ffeff9b5e513 217 MeasureBuf.push(measureGroup);
henryrawas 13:ffeff9b5e513 218 }
henryrawas 9:a0fb6c370dbb 219
henryrawas 9:a0fb6c370dbb 220 if (!ok)
henryrawas 9:a0fb6c370dbb 221 {
henryrawas 9:a0fb6c370dbb 222 // report HW error
henryrawas 9:a0fb6c370dbb 223 int partix = robotArm.GetLastErrorPart();
henryrawas 9:a0fb6c370dbb 224 int errCode = robotArm.GetLastError();
henryrawas 13:ffeff9b5e513 225 printf("Hardware error detected joint %d, code %d \r\n", partix, errCode);
henryrawas 13:ffeff9b5e513 226 PushHardwareAlert(partix, errCode);
henryrawas 9:a0fb6c370dbb 227 MainState = MS_Error;
henryrawas 9:a0fb6c370dbb 228 }
henryrawas 9:a0fb6c370dbb 229 }
henryrawas 8:d98e2dec0f40 230
henryrawas 10:9b21566a5ddb 231
henryrawas 10:9b21566a5ddb 232
henryrawas 7:6723f6887d00 233 void ControlArm(const char* cmd)
henryrawas 7:6723f6887d00 234 {
henryrawas 7:6723f6887d00 235 if (strncmp(cmd, "pause", 5) == 0)
henryrawas 7:6723f6887d00 236 {
henryrawas 10:9b21566a5ddb 237 ShowLedBlue();
henryrawas 7:6723f6887d00 238 MainState = MS_Paused;
henryrawas 7:6723f6887d00 239 }
henryrawas 7:6723f6887d00 240 else if (strncmp(cmd, "resume", 6) == 0)
henryrawas 7:6723f6887d00 241 {
henryrawas 10:9b21566a5ddb 242 ShowLedGreen();
henryrawas 13:ffeff9b5e513 243 MainState = MS_Resuming;
henryrawas 7:6723f6887d00 244 }
henryrawas 10:9b21566a5ddb 245 else if (strncmp(cmd, "cancel", 6) == 0)
henryrawas 10:9b21566a5ddb 246 {
henryrawas 10:9b21566a5ddb 247 ShowLedGreen();
henryrawas 10:9b21566a5ddb 248 MainState = MS_CancelOne;
henryrawas 10:9b21566a5ddb 249 }
henryrawas 7:6723f6887d00 250 else if (strncmp(cmd, "runupdown", 9) == 0)
henryrawas 7:6723f6887d00 251 {
henryrawas 10:9b21566a5ddb 252 ShowLedGreen();
henryrawas 13:ffeff9b5e513 253 if (MainState == MS_Idle || MainState == MS_Paused)
henryrawas 13:ffeff9b5e513 254 MainState = MS_Running;
henryrawas 7:6723f6887d00 255 SequenceQ.put(&UpDownSeq);
henryrawas 7:6723f6887d00 256 }
henryrawas 7:6723f6887d00 257 else if (strncmp(cmd, "runuptwist", 10) == 0)
henryrawas 7:6723f6887d00 258 {
henryrawas 10:9b21566a5ddb 259 ShowLedGreen();
henryrawas 13:ffeff9b5e513 260 if (MainState == MS_Idle || MainState == MS_Paused)
henryrawas 13:ffeff9b5e513 261 MainState = MS_Running;
henryrawas 7:6723f6887d00 262 SequenceQ.put(&UpTwistSeq);
henryrawas 7:6723f6887d00 263 }
henryrawas 7:6723f6887d00 264 else if (strncmp(cmd, "runhome", 10) == 0)
henryrawas 7:6723f6887d00 265 {
henryrawas 10:9b21566a5ddb 266 ShowLedGreen();
henryrawas 13:ffeff9b5e513 267 if (MainState == MS_Idle || MainState == MS_Paused)
henryrawas 13:ffeff9b5e513 268 MainState = MS_Running;
henryrawas 7:6723f6887d00 269 SequenceQ.put(&StartSeq);
henryrawas 7:6723f6887d00 270 }
henryrawas 7:6723f6887d00 271 else if (strncmp(cmd, "runwave", 10) == 0)
henryrawas 7:6723f6887d00 272 {
henryrawas 10:9b21566a5ddb 273 ShowLedGreen();
henryrawas 13:ffeff9b5e513 274 MainState = MS_Running;
henryrawas 7:6723f6887d00 275 SequenceQ.put(&WaveSeq);
henryrawas 7:6723f6887d00 276 }
henryrawas 8:d98e2dec0f40 277 else if (strncmp(cmd, "runtaps1", 10) == 0)
henryrawas 8:d98e2dec0f40 278 {
henryrawas 10:9b21566a5ddb 279 ShowLedGreen();
henryrawas 13:ffeff9b5e513 280 if (MainState == MS_Idle || MainState == MS_Paused)
henryrawas 13:ffeff9b5e513 281 MainState = MS_Running;
henryrawas 8:d98e2dec0f40 282 SequenceQ.put(&TapsSeq);
henryrawas 8:d98e2dec0f40 283 }
henryrawas 10:9b21566a5ddb 284 else if (strncmp(cmd, "fastwave", 8) == 0)
henryrawas 7:6723f6887d00 285 {
henryrawas 10:9b21566a5ddb 286 ShowLedGreen();
henryrawas 13:ffeff9b5e513 287 MainState = MS_Running;
henryrawas 13:ffeff9b5e513 288 SequenceQ.put(&BigWaveSeq);
henryrawas 13:ffeff9b5e513 289 }
henryrawas 13:ffeff9b5e513 290 else if (strncmp(cmd, "beep", 9) == 0)
henryrawas 13:ffeff9b5e513 291 {
henryrawas 13:ffeff9b5e513 292 BuzzerStartMs((int)IdleTimer.read_ms(), 500);
henryrawas 7:6723f6887d00 293 }
henryrawas 7:6723f6887d00 294 }
henryrawas 7:6723f6887d00 295
henryrawas 13:ffeff9b5e513 296
henryrawas 13:ffeff9b5e513 297 // things to initialize early
henryrawas 13:ffeff9b5e513 298 void PrepareController()
henryrawas 7:6723f6887d00 299 {
henryrawas 13:ffeff9b5e513 300 MakeSequences(NUMJOINTS);
henryrawas 7:6723f6887d00 301 }
henryrawas 7:6723f6887d00 302
henryrawas 13:ffeff9b5e513 303 // Run the controller thread loop
henryrawas 7:6723f6887d00 304 void RunController()
henryrawas 7:6723f6887d00 305 {
henryrawas 13:ffeff9b5e513 306 printf("RunController start");
henryrawas 13:ffeff9b5e513 307
henryrawas 7:6723f6887d00 308 // init robot arm
henryrawas 7:6723f6887d00 309 RobotArm robotArm;
henryrawas 13:ffeff9b5e513 310 robotArm.SetStepMs(CONTROLLER_POLL);
henryrawas 7:6723f6887d00 311
henryrawas 7:6723f6887d00 312 int partSize = robotArm.GetNumParts();
henryrawas 7:6723f6887d00 313
henryrawas 10:9b21566a5ddb 314 IdleTimer.start();
henryrawas 10:9b21566a5ddb 315
henryrawas 13:ffeff9b5e513 316 // set running state
henryrawas 7:6723f6887d00 317 MainState = MS_Idle;
henryrawas 7:6723f6887d00 318 RunInSequence = false;
henryrawas 7:6723f6887d00 319 RunInMoveStep = false;
henryrawas 7:6723f6887d00 320
henryrawas 13:ffeff9b5e513 321 NeedHwReset = false;
henryrawas 7:6723f6887d00 322
henryrawas 13:ffeff9b5e513 323 // set first sequence to run
henryrawas 7:6723f6887d00 324 SequenceQ.put(&StartSeq);
henryrawas 7:6723f6887d00 325
henryrawas 7:6723f6887d00 326 // state for sequence
henryrawas 13:ffeff9b5e513 327 vector<ActionSequence*> *curseq = NULL;
henryrawas 7:6723f6887d00 328 int curseqIx = 0;
henryrawas 10:9b21566a5ddb 329 int loopSeqIx = 0;
henryrawas 10:9b21566a5ddb 330 int loopCounter = 0;
henryrawas 10:9b21566a5ddb 331
henryrawas 13:ffeff9b5e513 332 // init time outs
henryrawas 13:ffeff9b5e513 333 NextStepMs = 0;
henryrawas 13:ffeff9b5e513 334 NextPollMs = (int)IdleTimer.read_ms();
henryrawas 13:ffeff9b5e513 335 NextSendMs = (int)IdleTimer.read_ms() + SEND_STATUS_TO;
henryrawas 7:6723f6887d00 336
henryrawas 7:6723f6887d00 337 MainState = MS_Running;
henryrawas 13:ffeff9b5e513 338 NodeMeasure nextMetric = NM_Temperature;
henryrawas 7:6723f6887d00 339
henryrawas 7:6723f6887d00 340 while( true )
henryrawas 7:6723f6887d00 341 {
henryrawas 13:ffeff9b5e513 342 int now = (int)IdleTimer.read_ms();
henryrawas 7:6723f6887d00 343
henryrawas 13:ffeff9b5e513 344 // Sleep until next controller poll time
henryrawas 13:ffeff9b5e513 345 if (NextPollMs > now)
henryrawas 13:ffeff9b5e513 346 {
henryrawas 13:ffeff9b5e513 347 ThreadAPI_Sleep(NextPollMs - now);
henryrawas 13:ffeff9b5e513 348 now = (int)IdleTimer.read_ms();
henryrawas 13:ffeff9b5e513 349 }
henryrawas 13:ffeff9b5e513 350 else
henryrawas 11:3a2e6eb9fbb8 351 {
henryrawas 13:ffeff9b5e513 352 printf("too slow %d\r\n", now - NextPollMs);
henryrawas 13:ffeff9b5e513 353 }
henryrawas 13:ffeff9b5e513 354 // set next poll time
henryrawas 13:ffeff9b5e513 355 NextPollMs = now + CONTROLLER_POLL;
henryrawas 13:ffeff9b5e513 356
henryrawas 13:ffeff9b5e513 357 // if had HW error, reset error state
henryrawas 13:ffeff9b5e513 358 if (MainState != MS_Error)
henryrawas 13:ffeff9b5e513 359 {
henryrawas 13:ffeff9b5e513 360 if (NeedHwReset)
henryrawas 13:ffeff9b5e513 361 {
henryrawas 13:ffeff9b5e513 362 robotArm.ClearErrorState();
henryrawas 13:ffeff9b5e513 363 NeedHwReset = false;
henryrawas 13:ffeff9b5e513 364 }
henryrawas 11:3a2e6eb9fbb8 365 }
henryrawas 11:3a2e6eb9fbb8 366
henryrawas 7:6723f6887d00 367 switch (MainState)
henryrawas 7:6723f6887d00 368 {
henryrawas 7:6723f6887d00 369 case MS_Idle:
henryrawas 7:6723f6887d00 370 break;
henryrawas 7:6723f6887d00 371
henryrawas 7:6723f6887d00 372 case MS_Paused:
henryrawas 7:6723f6887d00 373 break;
henryrawas 7:6723f6887d00 374
henryrawas 7:6723f6887d00 375 case MS_Error:
henryrawas 7:6723f6887d00 376 break;
henryrawas 7:6723f6887d00 377
henryrawas 7:6723f6887d00 378 case MS_CancelOne:
henryrawas 7:6723f6887d00 379 RunInSequence = false;
henryrawas 7:6723f6887d00 380 RunInMoveStep = false;
henryrawas 13:ffeff9b5e513 381 robotArm.MoveArmPositionsEnd();
henryrawas 7:6723f6887d00 382 MainState = MS_Running;
henryrawas 13:ffeff9b5e513 383 // avoid next poll delay
henryrawas 13:ffeff9b5e513 384 NextPollMs = now;
henryrawas 7:6723f6887d00 385 break;
henryrawas 7:6723f6887d00 386
henryrawas 7:6723f6887d00 387 case MS_CancelAll:
henryrawas 7:6723f6887d00 388 RunInSequence = false;
henryrawas 7:6723f6887d00 389 RunInMoveStep = false;
henryrawas 13:ffeff9b5e513 390 robotArm.MoveArmPositionsEnd();
henryrawas 7:6723f6887d00 391 while (1) {
henryrawas 7:6723f6887d00 392 osEvent evt = SequenceQ.get(0);
henryrawas 7:6723f6887d00 393 if (evt.status != osEventMessage)
henryrawas 7:6723f6887d00 394 break;
henryrawas 7:6723f6887d00 395 }
henryrawas 13:ffeff9b5e513 396 // avoid next poll delay
henryrawas 13:ffeff9b5e513 397 NextPollMs = now;
henryrawas 13:ffeff9b5e513 398 MainState = MS_Running;
henryrawas 13:ffeff9b5e513 399 break;
henryrawas 13:ffeff9b5e513 400
henryrawas 13:ffeff9b5e513 401 case MS_Resuming:
henryrawas 13:ffeff9b5e513 402 robotArm.MoveArmPositionsResume();
henryrawas 13:ffeff9b5e513 403 // avoid next poll delay
henryrawas 13:ffeff9b5e513 404 NextPollMs = now;
henryrawas 7:6723f6887d00 405 MainState = MS_Running;
henryrawas 7:6723f6887d00 406 break;
henryrawas 7:6723f6887d00 407
henryrawas 7:6723f6887d00 408 case MS_Running:
henryrawas 7:6723f6887d00 409 if (!RunInSequence)
henryrawas 7:6723f6887d00 410 {
henryrawas 13:ffeff9b5e513 411 // start new sequence of actions
henryrawas 7:6723f6887d00 412 osEvent evt = SequenceQ.get(0);
henryrawas 7:6723f6887d00 413 if (evt.status == osEventMessage)
henryrawas 7:6723f6887d00 414 {
henryrawas 13:ffeff9b5e513 415 printf("New Seq\r\n");
henryrawas 13:ffeff9b5e513 416 curseq = (vector<ActionSequence*> *)evt.value.p;
henryrawas 7:6723f6887d00 417 curseqIx = 0;
henryrawas 7:6723f6887d00 418 RunInSequence = true;
henryrawas 7:6723f6887d00 419 RunInMoveStep = false;
henryrawas 7:6723f6887d00 420 }
henryrawas 13:ffeff9b5e513 421 else
henryrawas 13:ffeff9b5e513 422 {
henryrawas 13:ffeff9b5e513 423 MainState = MS_Idle;
henryrawas 13:ffeff9b5e513 424 }
henryrawas 7:6723f6887d00 425 }
henryrawas 7:6723f6887d00 426
henryrawas 7:6723f6887d00 427 if (RunInSequence)
henryrawas 7:6723f6887d00 428 {
henryrawas 7:6723f6887d00 429 if (RunInMoveStep)
henryrawas 7:6723f6887d00 430 {
henryrawas 13:ffeff9b5e513 431 // check if delaying next move
henryrawas 13:ffeff9b5e513 432 if (NextStepMs > now)
henryrawas 7:6723f6887d00 433 break;
henryrawas 13:ffeff9b5e513 434
henryrawas 7:6723f6887d00 435 if (robotArm.MoveArmPositionsHasNext())
henryrawas 7:6723f6887d00 436 {
henryrawas 7:6723f6887d00 437 bool ok = robotArm.MoveArmPositionsNext();
henryrawas 13:ffeff9b5e513 438 if (!ok)
henryrawas 7:6723f6887d00 439 {
henryrawas 7:6723f6887d00 440 // report HW error
henryrawas 8:d98e2dec0f40 441 int partix = robotArm.GetLastErrorPart();
henryrawas 8:d98e2dec0f40 442 int errCode = robotArm.GetLastError();
henryrawas 13:ffeff9b5e513 443 printf("Hardware error detected joint %d, code %d \r\n", partix, errCode);
henryrawas 13:ffeff9b5e513 444 PushHardwareAlert(partix, errCode);
henryrawas 7:6723f6887d00 445 MainState = MS_Error;
henryrawas 8:d98e2dec0f40 446 break;
henryrawas 7:6723f6887d00 447 }
henryrawas 7:6723f6887d00 448 }
henryrawas 7:6723f6887d00 449 else
henryrawas 7:6723f6887d00 450 {
henryrawas 8:d98e2dec0f40 451 printf("No more Step\r\n");
henryrawas 7:6723f6887d00 452 RunInMoveStep = false;
henryrawas 7:6723f6887d00 453 }
henryrawas 7:6723f6887d00 454 }
henryrawas 7:6723f6887d00 455 if (!RunInMoveStep)
henryrawas 7:6723f6887d00 456 {
henryrawas 7:6723f6887d00 457 if (curseq != NULL)
henryrawas 7:6723f6887d00 458 {
henryrawas 7:6723f6887d00 459 if (curseqIx >= curseq->size())
henryrawas 7:6723f6887d00 460 {
henryrawas 10:9b21566a5ddb 461 printf("sequence completed. Stopping\r\n");
henryrawas 7:6723f6887d00 462 RunInSequence = false;
henryrawas 7:6723f6887d00 463 break;
henryrawas 7:6723f6887d00 464 }
henryrawas 7:6723f6887d00 465
henryrawas 13:ffeff9b5e513 466 ActionSequence* aseq = (*curseq)[curseqIx];
henryrawas 7:6723f6887d00 467 curseqIx++;
henryrawas 7:6723f6887d00 468
henryrawas 13:ffeff9b5e513 469 switch (aseq->ActionType)
henryrawas 7:6723f6887d00 470 {
henryrawas 7:6723f6887d00 471 case SA_SetGoal:
henryrawas 8:d98e2dec0f40 472 printf(" - Move arm start\r\n");
henryrawas 13:ffeff9b5e513 473 robotArm.MoveArmPositionsStart(aseq->GoalVals, aseq->Param);
henryrawas 8:d98e2dec0f40 474 RunInMoveStep = true;
henryrawas 7:6723f6887d00 475 break;
henryrawas 7:6723f6887d00 476 case SA_Delay:
henryrawas 8:d98e2dec0f40 477 printf(" - Delay\r\n");
henryrawas 13:ffeff9b5e513 478 NextStepMs = aseq->Param + now;
henryrawas 7:6723f6887d00 479 break;
henryrawas 10:9b21566a5ddb 480 case SA_LoopBegin:
henryrawas 10:9b21566a5ddb 481 printf(" - LoopBegin\r\n");
henryrawas 10:9b21566a5ddb 482 loopSeqIx = curseqIx;
henryrawas 13:ffeff9b5e513 483 loopCounter = aseq->Param;
henryrawas 7:6723f6887d00 484 break;
henryrawas 10:9b21566a5ddb 485 case SA_LoopEnd:
henryrawas 10:9b21566a5ddb 486 printf(" - LoopEnd\r\n");
henryrawas 10:9b21566a5ddb 487 loopCounter--;
henryrawas 10:9b21566a5ddb 488 if (loopCounter > 0 && loopSeqIx > 0)
henryrawas 10:9b21566a5ddb 489 curseqIx = loopSeqIx;
henryrawas 10:9b21566a5ddb 490 break;
henryrawas 7:6723f6887d00 491 }
henryrawas 7:6723f6887d00 492 }
henryrawas 7:6723f6887d00 493 }
henryrawas 7:6723f6887d00 494
henryrawas 7:6723f6887d00 495 }
henryrawas 7:6723f6887d00 496 break;
henryrawas 7:6723f6887d00 497 }
henryrawas 13:ffeff9b5e513 498
henryrawas 13:ffeff9b5e513 499 bool sendAlert = MainState != MS_Error;
henryrawas 13:ffeff9b5e513 500
henryrawas 13:ffeff9b5e513 501 // get measurements and test thresholds
henryrawas 13:ffeff9b5e513 502 // Reading all values takes too long,
henryrawas 13:ffeff9b5e513 503 // so we read the load and 1 other value each time
henryrawas 13:ffeff9b5e513 504 int rc = 0;
henryrawas 13:ffeff9b5e513 505 rc = robotArm.ArmMeasuresTest(NM_Load);
henryrawas 13:ffeff9b5e513 506 if (rc < 0 && sendAlert)
henryrawas 13:ffeff9b5e513 507 {
henryrawas 13:ffeff9b5e513 508 PushLoadAlert(robotArm);
henryrawas 13:ffeff9b5e513 509 MainState = MS_Error;
henryrawas 13:ffeff9b5e513 510 }
henryrawas 13:ffeff9b5e513 511
henryrawas 13:ffeff9b5e513 512 switch (nextMetric)
henryrawas 13:ffeff9b5e513 513 {
henryrawas 13:ffeff9b5e513 514 case NM_Temperature:
henryrawas 13:ffeff9b5e513 515 rc = robotArm.ArmMeasuresTest(NM_Temperature);
henryrawas 13:ffeff9b5e513 516 if (rc < 0 && sendAlert)
henryrawas 13:ffeff9b5e513 517 {
henryrawas 13:ffeff9b5e513 518 PushTemperatureAlert(robotArm);
henryrawas 13:ffeff9b5e513 519 MainState = MS_Error;
henryrawas 13:ffeff9b5e513 520 }
henryrawas 13:ffeff9b5e513 521 nextMetric = NM_Voltage;
henryrawas 13:ffeff9b5e513 522 break;
henryrawas 13:ffeff9b5e513 523
henryrawas 13:ffeff9b5e513 524 case NM_Voltage:
henryrawas 13:ffeff9b5e513 525 rc = robotArm.ArmMeasuresTest(NM_Voltage);
henryrawas 13:ffeff9b5e513 526 if (rc < 0 && sendAlert)
henryrawas 13:ffeff9b5e513 527 {
henryrawas 13:ffeff9b5e513 528 PushVoltageAlert(robotArm);
henryrawas 13:ffeff9b5e513 529 MainState = MS_Error;
henryrawas 13:ffeff9b5e513 530 }
henryrawas 13:ffeff9b5e513 531 nextMetric = NM_Degrees;
henryrawas 13:ffeff9b5e513 532 break;
henryrawas 13:ffeff9b5e513 533
henryrawas 13:ffeff9b5e513 534
henryrawas 13:ffeff9b5e513 535 case NM_Degrees:
henryrawas 13:ffeff9b5e513 536 rc = robotArm.ArmMeasuresTest(NM_Degrees);
henryrawas 13:ffeff9b5e513 537 if (rc < 0 && sendAlert)
henryrawas 13:ffeff9b5e513 538 {
henryrawas 13:ffeff9b5e513 539 PushPositionAlert(robotArm);
henryrawas 13:ffeff9b5e513 540 MainState = MS_Error;
henryrawas 13:ffeff9b5e513 541 }
henryrawas 13:ffeff9b5e513 542 nextMetric = NM_Temperature;
henryrawas 13:ffeff9b5e513 543 break;
henryrawas 7:6723f6887d00 544
henryrawas 13:ffeff9b5e513 545 default:
henryrawas 13:ffeff9b5e513 546 nextMetric = NM_Temperature;
henryrawas 13:ffeff9b5e513 547 break;
henryrawas 13:ffeff9b5e513 548 }
henryrawas 13:ffeff9b5e513 549 if (rc == -2)
henryrawas 10:9b21566a5ddb 550 {
henryrawas 13:ffeff9b5e513 551 int partix = robotArm.GetLastErrorPart();
henryrawas 13:ffeff9b5e513 552 int errCode = robotArm.GetLastError();
henryrawas 13:ffeff9b5e513 553 printf("Hardware error detected joint %d, code %d \r\n", partix, errCode);
henryrawas 13:ffeff9b5e513 554 PushHardwareAlert(partix, errCode);
henryrawas 13:ffeff9b5e513 555 MainState = MS_Error;
henryrawas 13:ffeff9b5e513 556 }
henryrawas 13:ffeff9b5e513 557
henryrawas 13:ffeff9b5e513 558 // check if buzzer needs to be turned off
henryrawas 13:ffeff9b5e513 559 BuzzerPoll(now);
henryrawas 13:ffeff9b5e513 560
henryrawas 13:ffeff9b5e513 561 // check if time to send status
henryrawas 13:ffeff9b5e513 562 if (now >= NextSendMs)
henryrawas 13:ffeff9b5e513 563 {
henryrawas 13:ffeff9b5e513 564 // if paused, use longer time out for sending data
henryrawas 13:ffeff9b5e513 565 if (MainState != MS_Paused ||
henryrawas 13:ffeff9b5e513 566 now > NextSendMs + SEND_STATUS_PAUSED_TO)
henryrawas 13:ffeff9b5e513 567 {
henryrawas 13:ffeff9b5e513 568 NextSendMs = now + SEND_STATUS_TO;
henryrawas 13:ffeff9b5e513 569 PushMeasurements(partSize, robotArm);
henryrawas 13:ffeff9b5e513 570 }
henryrawas 10:9b21566a5ddb 571 }
henryrawas 7:6723f6887d00 572 }
henryrawas 7:6723f6887d00 573 }
henryrawas 7:6723f6887d00 574