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@12:ac6c9d7f8c40, 2016-01-07 (annotated)

Committer:
henryrawas
Date:
Thu Jan 07 17:31:23 2016 +0000
Revision:
12:ac6c9d7f8c40
Parent:
11:3a2e6eb9fbb8
Child:
13:ffeff9b5e513
refactor robotnode

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