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@8:d98e2dec0f40, 2015-12-31 (annotated)

Committer:
henryrawas
Date:
Thu Dec 31 17:47:55 2015 +0000
Revision:
8:d98e2dec0f40
Parent:
7:6723f6887d00
Child:
9:a0fb6c370dbb
add taps

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