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?
User | Revision | Line number | New 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 |