demo project

Dependencies:   AX-12A Dynamixel mbed iothub_client EthernetInterface NTPClient ConfigFile SDFileSystem iothub_amqp_transport mbed-rtos proton-c-mbed wolfSSL

Committer:
henryrawas
Date:
Wed Jan 06 22:25:51 2016 +0000
Revision:
11:3a2e6eb9fbb8
Parent:
10:9b21566a5ddb
Child:
12:ac6c9d7f8c40
error reset. Better sequences;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
henryrawas 4:36a4eceb1b7f 1 #include "mbed.h"
henryrawas 4:36a4eceb1b7f 2 #include "rtos.h"
henryrawas 4:36a4eceb1b7f 3
henryrawas 4:36a4eceb1b7f 4 #include <DynamixelBus.h>
henryrawas 4:36a4eceb1b7f 5 #include <AX12.h>
henryrawas 4:36a4eceb1b7f 6 #include <Terminal.h>
henryrawas 4:36a4eceb1b7f 7 #include <vector>
henryrawas 4:36a4eceb1b7f 8 #include <RobotArm.h>
henryrawas 4:36a4eceb1b7f 9
henryrawas 4:36a4eceb1b7f 10 using namespace std;
henryrawas 4:36a4eceb1b7f 11
henryrawas 11:3a2e6eb9fbb8 12 DynamixelBus dynbus( PTC17, PTC16, D7, D6, 500000 );
henryrawas 4:36a4eceb1b7f 13
henryrawas 4:36a4eceb1b7f 14
henryrawas 4:36a4eceb1b7f 15 // set the id for each part in the chain, in order
henryrawas 10:9b21566a5ddb 16 #define NUMPARTS 5
henryrawas 5:36916b1c5a06 17 int PartIds[] = { 2, 3, 4, 6, 1 };
henryrawas 4:36a4eceb1b7f 18
henryrawas 7:6723f6887d00 19 #define FailMsLimit 200
henryrawas 7:6723f6887d00 20
henryrawas 4:36a4eceb1b7f 21 RobotArm::RobotArm()
henryrawas 4:36a4eceb1b7f 22 {
henryrawas 4:36a4eceb1b7f 23 // build arm
henryrawas 10:9b21566a5ddb 24 for (int i = 0; i < NUMPARTS; i++)
henryrawas 10:9b21566a5ddb 25 {
henryrawas 10:9b21566a5ddb 26 AX12* servo = new AX12(&dynbus, PartIds[i]);
henryrawas 10:9b21566a5ddb 27 servo->TorqueEnable(false);
henryrawas 10:9b21566a5ddb 28 servo->TorqueEnable(true);
henryrawas 10:9b21566a5ddb 29 _armParts[i] = dynamic_cast<RobotNode*>(servo);
henryrawas 10:9b21566a5ddb 30 }
henryrawas 10:9b21566a5ddb 31 _numParts = NUMPARTS;
henryrawas 4:36a4eceb1b7f 32
henryrawas 4:36a4eceb1b7f 33 numsteps = 0;
henryrawas 4:36a4eceb1b7f 34 _stepms = 20; // move every 20 ms
henryrawas 10:9b21566a5ddb 35 allowance = 2.0f; // allow 2 degree diff for position error
henryrawas 7:6723f6887d00 36 failms = 0;
henryrawas 4:36a4eceb1b7f 37 }
henryrawas 4:36a4eceb1b7f 38
henryrawas 11:3a2e6eb9fbb8 39 void RobotArm::ClearErrorState()
henryrawas 11:3a2e6eb9fbb8 40 {
henryrawas 11:3a2e6eb9fbb8 41 for (int i = 0; i < _numParts; i++)
henryrawas 11:3a2e6eb9fbb8 42 {
henryrawas 11:3a2e6eb9fbb8 43 _armParts[i]->DoAction(NA_ClearError, 0.0f);
henryrawas 11:3a2e6eb9fbb8 44 }
henryrawas 11:3a2e6eb9fbb8 45 }
henryrawas 4:36a4eceb1b7f 46
henryrawas 4:36a4eceb1b7f 47 // move all parts to specified postions in ms time
henryrawas 4:36a4eceb1b7f 48 bool RobotArm::MoveArmPositionsStart(vector<float> positions, int totms)
henryrawas 4:36a4eceb1b7f 49 {
henryrawas 4:36a4eceb1b7f 50 _lastErrorPart = 0;
henryrawas 4:36a4eceb1b7f 51
henryrawas 4:36a4eceb1b7f 52 MoveArmPositionsEnd();
henryrawas 5:36916b1c5a06 53 GetArmPositions(lastpos);
henryrawas 7:6723f6887d00 54 lastgoals.clear();
henryrawas 8:d98e2dec0f40 55 endgoals.clear();
henryrawas 8:d98e2dec0f40 56
henryrawas 7:6723f6887d00 57 numsteps = totms / _stepms;
henryrawas 7:6723f6887d00 58 if (numsteps == 0) numsteps = 1;
henryrawas 7:6723f6887d00 59
henryrawas 4:36a4eceb1b7f 60 differentials.clear();
henryrawas 8:d98e2dec0f40 61 for (int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 62 {
henryrawas 7:6723f6887d00 63 if (positions[ix] > 0.0f)
henryrawas 7:6723f6887d00 64 {
henryrawas 8:d98e2dec0f40 65 endgoals.push_back(positions[ix]);
henryrawas 7:6723f6887d00 66 float difference = (positions[ix] - lastpos[ix]) / (float)numsteps;
henryrawas 8:d98e2dec0f40 67 differentials.push_back(difference);
henryrawas 7:6723f6887d00 68 }
henryrawas 7:6723f6887d00 69 else
henryrawas 7:6723f6887d00 70 {
henryrawas 7:6723f6887d00 71 // negative goal. Treat as don't move
henryrawas 8:d98e2dec0f40 72 endgoals.push_back(lastpos[ix]);
henryrawas 7:6723f6887d00 73 differentials.push_back(0.0f);
henryrawas 7:6723f6887d00 74 }
henryrawas 4:36a4eceb1b7f 75 }
henryrawas 4:36a4eceb1b7f 76
henryrawas 4:36a4eceb1b7f 77 curstep = 1;
henryrawas 4:36a4eceb1b7f 78
henryrawas 4:36a4eceb1b7f 79 delayms = _stepms;
henryrawas 4:36a4eceb1b7f 80
henryrawas 4:36a4eceb1b7f 81 elapseTimer.start();
henryrawas 5:36916b1c5a06 82 expDelay = (int)elapseTimer.read_ms() + delayms;
henryrawas 7:6723f6887d00 83 failms = 0;
henryrawas 4:36a4eceb1b7f 84
henryrawas 4:36a4eceb1b7f 85 return true;
henryrawas 4:36a4eceb1b7f 86 }
henryrawas 4:36a4eceb1b7f 87
henryrawas 4:36a4eceb1b7f 88 bool RobotArm::MoveArmPositionsHasNext()
henryrawas 4:36a4eceb1b7f 89 {
henryrawas 4:36a4eceb1b7f 90 return (curstep <= numsteps);
henryrawas 4:36a4eceb1b7f 91 }
henryrawas 4:36a4eceb1b7f 92
henryrawas 7:6723f6887d00 93 bool RobotArm::MoveArmPositionsNext()
henryrawas 4:36a4eceb1b7f 94 {
henryrawas 4:36a4eceb1b7f 95 _lastErrorPart = 0;
henryrawas 4:36a4eceb1b7f 96
henryrawas 4:36a4eceb1b7f 97 if (curstep > numsteps)
henryrawas 4:36a4eceb1b7f 98 {
henryrawas 4:36a4eceb1b7f 99 // no more steps
henryrawas 4:36a4eceb1b7f 100 MoveArmPositionsEnd();
henryrawas 4:36a4eceb1b7f 101 return true;
henryrawas 4:36a4eceb1b7f 102 }
henryrawas 4:36a4eceb1b7f 103
henryrawas 4:36a4eceb1b7f 104 bool ok = true;
henryrawas 7:6723f6887d00 105
henryrawas 5:36916b1c5a06 106 lastgoals.clear();
henryrawas 8:d98e2dec0f40 107 for (int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 108 {
henryrawas 4:36a4eceb1b7f 109 if (_armParts[ix]->HasAction(NA_Rotate))
henryrawas 4:36a4eceb1b7f 110 {
henryrawas 8:d98e2dec0f40 111 float goal = (curstep == numsteps) ?
henryrawas 8:d98e2dec0f40 112 endgoals[ix] : // last step - use actual goal
henryrawas 8:d98e2dec0f40 113 (lastpos[ix] + (differentials[ix] * (float)curstep));
henryrawas 5:36916b1c5a06 114 lastgoals.push_back(goal);
henryrawas 7:6723f6887d00 115 if (differentials[ix] != 0.0f)
henryrawas 4:36a4eceb1b7f 116 {
henryrawas 7:6723f6887d00 117 bool ok = _armParts[ix]->DoAction(NA_Rotate, goal);
henryrawas 7:6723f6887d00 118 if (!ok)
henryrawas 7:6723f6887d00 119 {
henryrawas 7:6723f6887d00 120 _lastErrorPart = ix;
henryrawas 7:6723f6887d00 121 _lastError = _armParts[_lastErrorPart]->GetLastError();
henryrawas 7:6723f6887d00 122 _lastPosDiff = 0;
henryrawas 7:6723f6887d00 123 break;
henryrawas 7:6723f6887d00 124 }
henryrawas 4:36a4eceb1b7f 125 }
henryrawas 4:36a4eceb1b7f 126 }
henryrawas 4:36a4eceb1b7f 127 }
henryrawas 4:36a4eceb1b7f 128
henryrawas 4:36a4eceb1b7f 129 if (!ok)
henryrawas 4:36a4eceb1b7f 130 {
henryrawas 4:36a4eceb1b7f 131 return false;
henryrawas 4:36a4eceb1b7f 132 }
henryrawas 4:36a4eceb1b7f 133
henryrawas 4:36a4eceb1b7f 134 curstep++;
henryrawas 4:36a4eceb1b7f 135 if (curstep > numsteps)
henryrawas 4:36a4eceb1b7f 136 {
henryrawas 4:36a4eceb1b7f 137 MoveArmPositionsEnd();
henryrawas 4:36a4eceb1b7f 138 }
henryrawas 7:6723f6887d00 139
henryrawas 7:6723f6887d00 140 return true;
henryrawas 7:6723f6887d00 141 }
henryrawas 7:6723f6887d00 142
henryrawas 7:6723f6887d00 143 // calculate actual delay until expDelay
henryrawas 7:6723f6887d00 144 bool RobotArm::MoveArmPositionsDelay(int& nextdelay)
henryrawas 7:6723f6887d00 145 {
henryrawas 7:6723f6887d00 146 if (curstep <= numsteps)
henryrawas 7:6723f6887d00 147 {
henryrawas 4:36a4eceb1b7f 148 int elapsed = (int)elapseTimer.read_ms();
henryrawas 4:36a4eceb1b7f 149
henryrawas 7:6723f6887d00 150 if (elapsed <= expDelay)
henryrawas 4:36a4eceb1b7f 151 {
henryrawas 5:36916b1c5a06 152 if (expDelay - elapsed < delayms)
henryrawas 5:36916b1c5a06 153 nextdelay = expDelay - elapsed;
henryrawas 4:36a4eceb1b7f 154 else
henryrawas 4:36a4eceb1b7f 155 nextdelay = delayms;
henryrawas 7:6723f6887d00 156 // set next expected time by adding step delay
henryrawas 7:6723f6887d00 157 expDelay += delayms;
henryrawas 4:36a4eceb1b7f 158 }
henryrawas 4:36a4eceb1b7f 159 else
henryrawas 4:36a4eceb1b7f 160 {
henryrawas 7:6723f6887d00 161 nextdelay = delayms;
henryrawas 7:6723f6887d00 162 // set next expected time to now plus step delay
henryrawas 7:6723f6887d00 163 expDelay = elapsed + delayms;
henryrawas 4:36a4eceb1b7f 164 }
henryrawas 7:6723f6887d00 165 }
henryrawas 7:6723f6887d00 166 else
henryrawas 7:6723f6887d00 167 {
henryrawas 8:d98e2dec0f40 168 nextdelay = delayms;
henryrawas 4:36a4eceb1b7f 169 }
henryrawas 4:36a4eceb1b7f 170
henryrawas 4:36a4eceb1b7f 171 return true;
henryrawas 4:36a4eceb1b7f 172 }
henryrawas 4:36a4eceb1b7f 173
henryrawas 7:6723f6887d00 174 bool RobotArm::MoveArmPositionTest()
henryrawas 5:36916b1c5a06 175 {
henryrawas 5:36916b1c5a06 176 vector<float> curpos;
henryrawas 5:36916b1c5a06 177 GetArmPositions(curpos);
henryrawas 8:d98e2dec0f40 178 for (int ix = 0; ix < _numParts; ix++)
henryrawas 5:36916b1c5a06 179 {
henryrawas 7:6723f6887d00 180 if (curpos[ix] > 0 && lastgoals.size() > ix)
henryrawas 5:36916b1c5a06 181 {
henryrawas 7:6723f6887d00 182 float diff = fabs(curpos[ix] - lastgoals[ix]);
henryrawas 7:6723f6887d00 183 if (diff > (fabs(differentials[ix] * 2.0f) + allowance))
henryrawas 7:6723f6887d00 184 {
henryrawas 7:6723f6887d00 185 printf("Bad position for %d. Expect %f - got %f\r\n", ix, lastgoals[ix], curpos[ix]);
henryrawas 7:6723f6887d00 186 _lastPosDiff = diff;
henryrawas 7:6723f6887d00 187 _lastErrorPart = ix;
henryrawas 7:6723f6887d00 188 _lastError = 0;
henryrawas 7:6723f6887d00 189
henryrawas 7:6723f6887d00 190 int elapsed = (int)elapseTimer.read_ms();
henryrawas 7:6723f6887d00 191 if (failms > 0)
henryrawas 7:6723f6887d00 192 {
henryrawas 7:6723f6887d00 193 if (elapsed - failms > FailMsLimit)
henryrawas 7:6723f6887d00 194 {
henryrawas 7:6723f6887d00 195 // continuous failure for time period
henryrawas 7:6723f6887d00 196 // return false
henryrawas 7:6723f6887d00 197 lastgoals.clear();
henryrawas 7:6723f6887d00 198 failms = 0;
henryrawas 7:6723f6887d00 199 return false;
henryrawas 7:6723f6887d00 200 }
henryrawas 7:6723f6887d00 201 // within time period. Do not report failure
henryrawas 7:6723f6887d00 202 return true;
henryrawas 7:6723f6887d00 203 }
henryrawas 7:6723f6887d00 204 else
henryrawas 7:6723f6887d00 205 {
henryrawas 7:6723f6887d00 206 // first failure after success
henryrawas 7:6723f6887d00 207 // remember time. Do not report failure
henryrawas 7:6723f6887d00 208 failms = elapsed;
henryrawas 7:6723f6887d00 209 return true;
henryrawas 7:6723f6887d00 210 }
henryrawas 7:6723f6887d00 211 }
henryrawas 5:36916b1c5a06 212 }
henryrawas 5:36916b1c5a06 213 }
henryrawas 7:6723f6887d00 214 // success
henryrawas 7:6723f6887d00 215 failms = 0;
henryrawas 5:36916b1c5a06 216 return true;
henryrawas 5:36916b1c5a06 217 }
henryrawas 5:36916b1c5a06 218
henryrawas 4:36a4eceb1b7f 219 bool RobotArm::MoveArmPositionsEnd()
henryrawas 4:36a4eceb1b7f 220 {
henryrawas 4:36a4eceb1b7f 221 if (numsteps > 0)
henryrawas 4:36a4eceb1b7f 222 {
henryrawas 4:36a4eceb1b7f 223 elapseTimer.stop();
henryrawas 4:36a4eceb1b7f 224 numsteps = 0;
henryrawas 4:36a4eceb1b7f 225 }
henryrawas 4:36a4eceb1b7f 226 return true;
henryrawas 4:36a4eceb1b7f 227 }
henryrawas 4:36a4eceb1b7f 228
henryrawas 4:36a4eceb1b7f 229
henryrawas 4:36a4eceb1b7f 230 // get all parts positions
henryrawas 4:36a4eceb1b7f 231 bool RobotArm::GetArmPositions(vector<float>& outPos)
henryrawas 4:36a4eceb1b7f 232 {
henryrawas 9:a0fb6c370dbb 233 bool ok = true;
henryrawas 4:36a4eceb1b7f 234 outPos.clear();
henryrawas 8:d98e2dec0f40 235 for (int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 236 {
henryrawas 4:36a4eceb1b7f 237 float pos = _armParts[ix]->GetMeasure(NM_Degrees);
henryrawas 8:d98e2dec0f40 238 outPos.push_back(pos);
henryrawas 9:a0fb6c370dbb 239 if (_armParts[ix]->HasError())
henryrawas 9:a0fb6c370dbb 240 {
henryrawas 9:a0fb6c370dbb 241 _lastErrorPart = ix;
henryrawas 9:a0fb6c370dbb 242 _lastError = _armParts[ix]->GetLastError();
henryrawas 9:a0fb6c370dbb 243 ok = false;
henryrawas 9:a0fb6c370dbb 244 }
henryrawas 4:36a4eceb1b7f 245 }
henryrawas 9:a0fb6c370dbb 246 return ok;
henryrawas 4:36a4eceb1b7f 247 }
henryrawas 4:36a4eceb1b7f 248
henryrawas 7:6723f6887d00 249 // get all parts last measured positions
henryrawas 4:36a4eceb1b7f 250 bool RobotArm::GetArmLastPositions(vector<float>& outPos)
henryrawas 4:36a4eceb1b7f 251 {
henryrawas 9:a0fb6c370dbb 252 bool ok = true;
henryrawas 4:36a4eceb1b7f 253 outPos.clear();
henryrawas 8:d98e2dec0f40 254 for (int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 255 {
henryrawas 4:36a4eceb1b7f 256 float pos = _armParts[ix]->GetLastMeasure(NM_Degrees);
henryrawas 8:d98e2dec0f40 257 outPos.push_back(pos);
henryrawas 9:a0fb6c370dbb 258 if (_armParts[ix]->HasError())
henryrawas 9:a0fb6c370dbb 259 {
henryrawas 9:a0fb6c370dbb 260 _lastErrorPart = ix;
henryrawas 9:a0fb6c370dbb 261 _lastError = _armParts[ix]->GetLastError();
henryrawas 9:a0fb6c370dbb 262 ok = false;
henryrawas 9:a0fb6c370dbb 263 }
henryrawas 4:36a4eceb1b7f 264 }
henryrawas 9:a0fb6c370dbb 265 return ok;
henryrawas 4:36a4eceb1b7f 266 }
henryrawas 4:36a4eceb1b7f 267
henryrawas 7:6723f6887d00 268 // get all parts measurements
henryrawas 8:d98e2dec0f40 269 bool RobotArm::GetArmMeasure(int measureId, vector<float>& outVals)
henryrawas 4:36a4eceb1b7f 270 {
henryrawas 9:a0fb6c370dbb 271 bool ok = true;
henryrawas 8:d98e2dec0f40 272 outVals.clear();
henryrawas 8:d98e2dec0f40 273 for (int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 274 {
henryrawas 8:d98e2dec0f40 275 float val = _armParts[ix]->GetMeasure(measureId);
henryrawas 8:d98e2dec0f40 276 outVals.push_back(val);
henryrawas 9:a0fb6c370dbb 277 if (_armParts[ix]->HasError())
henryrawas 9:a0fb6c370dbb 278 {
henryrawas 9:a0fb6c370dbb 279 _lastErrorPart = ix;
henryrawas 9:a0fb6c370dbb 280 _lastError = _armParts[ix]->GetLastError();
henryrawas 9:a0fb6c370dbb 281 ok = false;
henryrawas 9:a0fb6c370dbb 282 }
henryrawas 4:36a4eceb1b7f 283 }
henryrawas 9:a0fb6c370dbb 284 return ok;
henryrawas 4:36a4eceb1b7f 285 }
henryrawas 4:36a4eceb1b7f 286
henryrawas 7:6723f6887d00 287 // get all parts last measurements
henryrawas 8:d98e2dec0f40 288 bool RobotArm::GetArmLastMeasure(int measureId, vector<float>& outVals)
henryrawas 4:36a4eceb1b7f 289 {
henryrawas 9:a0fb6c370dbb 290 bool ok = true;
henryrawas 8:d98e2dec0f40 291 outVals.clear();
henryrawas 8:d98e2dec0f40 292 for (int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 293 {
henryrawas 8:d98e2dec0f40 294 float val = _armParts[ix]->GetLastMeasure(measureId);
henryrawas 8:d98e2dec0f40 295 outVals.push_back(val);
henryrawas 9:a0fb6c370dbb 296 if (_armParts[ix]->HasError())
henryrawas 9:a0fb6c370dbb 297 {
henryrawas 9:a0fb6c370dbb 298 _lastErrorPart = ix;
henryrawas 9:a0fb6c370dbb 299 _lastError = _armParts[ix]->GetLastError();
henryrawas 9:a0fb6c370dbb 300 ok = false;
henryrawas 9:a0fb6c370dbb 301 }
henryrawas 4:36a4eceb1b7f 302 }
henryrawas 9:a0fb6c370dbb 303 return ok;
henryrawas 4:36a4eceb1b7f 304 }
henryrawas 4:36a4eceb1b7f 305
henryrawas 4:36a4eceb1b7f 306 int RobotArm::GetNumParts()
henryrawas 4:36a4eceb1b7f 307 {
henryrawas 4:36a4eceb1b7f 308 return _numParts;
henryrawas 4:36a4eceb1b7f 309 }
henryrawas 4:36a4eceb1b7f 310
henryrawas 4:36a4eceb1b7f 311 void RobotArm::SetStepMs(int stepms)
henryrawas 4:36a4eceb1b7f 312 {
henryrawas 4:36a4eceb1b7f 313 if (stepms > 0 && stepms < 5000)
henryrawas 4:36a4eceb1b7f 314 _stepms = stepms;
henryrawas 4:36a4eceb1b7f 315 }
henryrawas 4:36a4eceb1b7f 316
henryrawas 4:36a4eceb1b7f 317 void RobotArm::SetThreadId(osThreadId tid)
henryrawas 4:36a4eceb1b7f 318 {
henryrawas 4:36a4eceb1b7f 319 _tid = tid;
henryrawas 4:36a4eceb1b7f 320 }
henryrawas 4:36a4eceb1b7f 321
henryrawas 4:36a4eceb1b7f 322 // get part by position
henryrawas 4:36a4eceb1b7f 323 RobotNode* RobotArm::GetArmPart(int partIx)
henryrawas 4:36a4eceb1b7f 324 {
henryrawas 4:36a4eceb1b7f 325 return _armParts[partIx];
henryrawas 4:36a4eceb1b7f 326 }
henryrawas 4:36a4eceb1b7f 327
henryrawas 7:6723f6887d00 328 int RobotArm::GetLastError()
henryrawas 4:36a4eceb1b7f 329 {
henryrawas 7:6723f6887d00 330 return _lastError;
henryrawas 7:6723f6887d00 331 }
henryrawas 7:6723f6887d00 332
henryrawas 7:6723f6887d00 333 float RobotArm::GetLastPosDiff()
henryrawas 7:6723f6887d00 334 {
henryrawas 7:6723f6887d00 335 return _lastPosDiff;
henryrawas 4:36a4eceb1b7f 336 }
henryrawas 4:36a4eceb1b7f 337
henryrawas 4:36a4eceb1b7f 338 int RobotArm::GetLastErrorPart()
henryrawas 4:36a4eceb1b7f 339 {
henryrawas 4:36a4eceb1b7f 340 return _lastErrorPart;
henryrawas 4:36a4eceb1b7f 341 }
henryrawas 7:6723f6887d00 342
henryrawas 7:6723f6887d00 343 void RobotArm::SetAllowance(float allow)
henryrawas 7:6723f6887d00 344 {
henryrawas 7:6723f6887d00 345 allowance = allow;
henryrawas 7:6723f6887d00 346 }