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

RobotArm.cpp@10:9b21566a5ddb, 2016-01-06 (annotated)

Committer:
henryrawas
Date:
Wed Jan 06 00:58:41 2016 +0000
Revision:
10:9b21566a5ddb
Parent:
9:a0fb6c370dbb
Child:
11:3a2e6eb9fbb8
Send status every minute. Add stress sequence. Use LED colors

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