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@7:6723f6887d00, 2015-12-29 (annotated)

Committer:
henryrawas
Date:
Tue Dec 29 23:31:28 2015 +0000
Revision:
7:6723f6887d00
Parent:
5:36916b1c5a06
Child:
8:d98e2dec0f40
motion block alerts, more commands

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 7:6723f6887d00 24 printf( "id[%d] = %d\r\n", i, PartIds[i]);
henryrawas 4:36a4eceb1b7f 25 AX12* servo1 = new AX12( &dynbus, PartIds[i] );
henryrawas 4:36a4eceb1b7f 26 servo1->TorqueEnable(true);
henryrawas 4:36a4eceb1b7f 27 _armParts[i] = dynamic_cast<RobotNode*>(servo1);
henryrawas 4:36a4eceb1b7f 28
henryrawas 4:36a4eceb1b7f 29 i++;
henryrawas 7:6723f6887d00 30 printf( "id[%d] = %d\r\n", i, PartIds[i]);
henryrawas 4:36a4eceb1b7f 31 AX12* servo2 = new AX12( &dynbus, PartIds[i] );
henryrawas 4:36a4eceb1b7f 32 servo2->TorqueEnable(true);
henryrawas 4:36a4eceb1b7f 33 _armParts[i] = dynamic_cast<RobotNode*>(servo2);
henryrawas 4:36a4eceb1b7f 34
henryrawas 4:36a4eceb1b7f 35 i++;
henryrawas 7:6723f6887d00 36 printf( "id[%d] = %d\r\n", i, PartIds[i]);
henryrawas 4:36a4eceb1b7f 37 AX12* servo3 = new AX12( &dynbus, PartIds[i] );
henryrawas 4:36a4eceb1b7f 38 servo3->TorqueEnable(true);
henryrawas 4:36a4eceb1b7f 39 _armParts[i] = dynamic_cast<RobotNode*>(servo3);
henryrawas 4:36a4eceb1b7f 40
henryrawas 4:36a4eceb1b7f 41 i++;
henryrawas 7:6723f6887d00 42 printf( "id[%d] = %d\r\n", i, PartIds[i]);
henryrawas 4:36a4eceb1b7f 43 AX12* servo4 = new AX12( &dynbus, PartIds[i] );
henryrawas 4:36a4eceb1b7f 44 servo4->TorqueEnable(true);
henryrawas 4:36a4eceb1b7f 45 _armParts[i] = dynamic_cast<RobotNode*>(servo4);
henryrawas 4:36a4eceb1b7f 46
henryrawas 4:36a4eceb1b7f 47 i++;
henryrawas 7:6723f6887d00 48 printf( "id[%d] = %d\r\n", i, PartIds[i]);
henryrawas 4:36a4eceb1b7f 49 AX12* servo6 = new AX12( &dynbus, PartIds[i] );
henryrawas 4:36a4eceb1b7f 50 servo6->TorqueEnable(true);
henryrawas 4:36a4eceb1b7f 51 _armParts[i] = dynamic_cast<RobotNode*>(servo6);
henryrawas 4:36a4eceb1b7f 52
henryrawas 4:36a4eceb1b7f 53 i++;
henryrawas 4:36a4eceb1b7f 54 _numParts = i;
henryrawas 4:36a4eceb1b7f 55
henryrawas 4:36a4eceb1b7f 56 numsteps = 0;
henryrawas 4:36a4eceb1b7f 57 _stepms = 20; // move every 20 ms
henryrawas 7:6723f6887d00 58 allowance = 2.0f; // allow 2 degree fudge factor
henryrawas 7:6723f6887d00 59 failms = 0;
henryrawas 4:36a4eceb1b7f 60 }
henryrawas 4:36a4eceb1b7f 61
henryrawas 4:36a4eceb1b7f 62
henryrawas 4:36a4eceb1b7f 63 // move all parts to specified postions in ms time
henryrawas 4:36a4eceb1b7f 64 bool RobotArm::MoveArmPositionsStart(vector<float> positions, int totms)
henryrawas 4:36a4eceb1b7f 65 {
henryrawas 4:36a4eceb1b7f 66 _lastErrorPart = 0;
henryrawas 4:36a4eceb1b7f 67
henryrawas 4:36a4eceb1b7f 68 MoveArmPositionsEnd();
henryrawas 5:36916b1c5a06 69 GetArmPositions(lastpos);
henryrawas 7:6723f6887d00 70 lastgoals.clear();
henryrawas 4:36a4eceb1b7f 71
henryrawas 7:6723f6887d00 72 numsteps = totms / _stepms;
henryrawas 7:6723f6887d00 73 if (numsteps == 0) numsteps = 1;
henryrawas 7:6723f6887d00 74
henryrawas 4:36a4eceb1b7f 75 differentials.clear();
henryrawas 4:36a4eceb1b7f 76 for( int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 77 {
henryrawas 7:6723f6887d00 78 if (positions[ix] > 0.0f)
henryrawas 7:6723f6887d00 79 {
henryrawas 7:6723f6887d00 80 float difference = (positions[ix] - lastpos[ix]) / (float)numsteps;
henryrawas 7:6723f6887d00 81 differentials.push_back( difference );
henryrawas 7:6723f6887d00 82 }
henryrawas 7:6723f6887d00 83 else
henryrawas 7:6723f6887d00 84 {
henryrawas 7:6723f6887d00 85 // negative goal. Treat as don't move
henryrawas 7:6723f6887d00 86 differentials.push_back(0.0f);
henryrawas 7:6723f6887d00 87 }
henryrawas 4:36a4eceb1b7f 88 }
henryrawas 4:36a4eceb1b7f 89
henryrawas 4:36a4eceb1b7f 90 curstep = 1;
henryrawas 4:36a4eceb1b7f 91
henryrawas 4:36a4eceb1b7f 92 delayms = _stepms;
henryrawas 4:36a4eceb1b7f 93
henryrawas 4:36a4eceb1b7f 94 elapseTimer.start();
henryrawas 5:36916b1c5a06 95 expDelay = (int)elapseTimer.read_ms() + delayms;
henryrawas 7:6723f6887d00 96 failms = 0;
henryrawas 4:36a4eceb1b7f 97
henryrawas 4:36a4eceb1b7f 98 return true;
henryrawas 4:36a4eceb1b7f 99 }
henryrawas 4:36a4eceb1b7f 100
henryrawas 4:36a4eceb1b7f 101 bool RobotArm::MoveArmPositionsHasNext()
henryrawas 4:36a4eceb1b7f 102 {
henryrawas 4:36a4eceb1b7f 103 return (curstep <= numsteps);
henryrawas 4:36a4eceb1b7f 104 }
henryrawas 4:36a4eceb1b7f 105
henryrawas 7:6723f6887d00 106 bool RobotArm::MoveArmPositionsNext()
henryrawas 4:36a4eceb1b7f 107 {
henryrawas 4:36a4eceb1b7f 108 _lastErrorPart = 0;
henryrawas 4:36a4eceb1b7f 109
henryrawas 4:36a4eceb1b7f 110 if (curstep > numsteps)
henryrawas 4:36a4eceb1b7f 111 {
henryrawas 4:36a4eceb1b7f 112 // no more steps
henryrawas 4:36a4eceb1b7f 113 MoveArmPositionsEnd();
henryrawas 4:36a4eceb1b7f 114 return true;
henryrawas 4:36a4eceb1b7f 115 }
henryrawas 4:36a4eceb1b7f 116
henryrawas 4:36a4eceb1b7f 117 bool ok = true;
henryrawas 7:6723f6887d00 118
henryrawas 5:36916b1c5a06 119 lastgoals.clear();
henryrawas 4:36a4eceb1b7f 120 for( int ix = 0; ix < _numParts; ix++ )
henryrawas 4:36a4eceb1b7f 121 {
henryrawas 4:36a4eceb1b7f 122 if (_armParts[ix]->HasAction(NA_Rotate))
henryrawas 4:36a4eceb1b7f 123 {
henryrawas 7:6723f6887d00 124 float goal = 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 7:6723f6887d00 179 nextdelay = 0;
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 5:36916b1c5a06 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 4:36a4eceb1b7f 245 for( int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 246 {
henryrawas 4:36a4eceb1b7f 247 float pos = _armParts[ix]->GetMeasure(NM_Degrees);
henryrawas 4:36a4eceb1b7f 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 4:36a4eceb1b7f 257 for( int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 258 {
henryrawas 4:36a4eceb1b7f 259 float pos = _armParts[ix]->GetLastMeasure(NM_Degrees);
henryrawas 4:36a4eceb1b7f 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 4:36a4eceb1b7f 266 bool RobotArm::GetArmMeasure(int measureId, vector<float>& outPos)
henryrawas 4:36a4eceb1b7f 267 {
henryrawas 4:36a4eceb1b7f 268 outPos.clear();
henryrawas 4:36a4eceb1b7f 269 for( int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 270 {
henryrawas 4:36a4eceb1b7f 271 float pos = _armParts[ix]->GetMeasure(measureId);
henryrawas 4:36a4eceb1b7f 272 outPos.push_back( pos );
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 4:36a4eceb1b7f 278 bool RobotArm::GetArmLastMeasure(int measureId, vector<float>& outPos)
henryrawas 4:36a4eceb1b7f 279 {
henryrawas 4:36a4eceb1b7f 280 outPos.clear();
henryrawas 4:36a4eceb1b7f 281 for( int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 282 {
henryrawas 4:36a4eceb1b7f 283 float pos = _armParts[ix]->GetLastMeasure(measureId);
henryrawas 4:36a4eceb1b7f 284 outPos.push_back( pos );
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 }