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

Committer:
henryrawas
Date:
Thu Dec 31 20:02:58 2015 +0000
Revision:
9:a0fb6c370dbb
Parent:
8:d98e2dec0f40
Child:
10:9b21566a5ddb
report errors

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 9:a0fb6c370dbb 244 bool ok = true;
henryrawas 4:36a4eceb1b7f 245 outPos.clear();
henryrawas 8:d98e2dec0f40 246 for (int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 247 {
henryrawas 4:36a4eceb1b7f 248 float pos = _armParts[ix]->GetMeasure(NM_Degrees);
henryrawas 8:d98e2dec0f40 249 outPos.push_back(pos);
henryrawas 9:a0fb6c370dbb 250 if (_armParts[ix]->HasError())
henryrawas 9:a0fb6c370dbb 251 {
henryrawas 9:a0fb6c370dbb 252 _lastErrorPart = ix;
henryrawas 9:a0fb6c370dbb 253 _lastError = _armParts[ix]->GetLastError();
henryrawas 9:a0fb6c370dbb 254 ok = false;
henryrawas 9:a0fb6c370dbb 255 }
henryrawas 4:36a4eceb1b7f 256 }
henryrawas 9:a0fb6c370dbb 257 return ok;
henryrawas 4:36a4eceb1b7f 258 }
henryrawas 4:36a4eceb1b7f 259
henryrawas 7:6723f6887d00 260 // get all parts last measured positions
henryrawas 4:36a4eceb1b7f 261 bool RobotArm::GetArmLastPositions(vector<float>& outPos)
henryrawas 4:36a4eceb1b7f 262 {
henryrawas 9:a0fb6c370dbb 263 bool ok = true;
henryrawas 4:36a4eceb1b7f 264 outPos.clear();
henryrawas 8:d98e2dec0f40 265 for (int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 266 {
henryrawas 4:36a4eceb1b7f 267 float pos = _armParts[ix]->GetLastMeasure(NM_Degrees);
henryrawas 8:d98e2dec0f40 268 outPos.push_back(pos);
henryrawas 9:a0fb6c370dbb 269 if (_armParts[ix]->HasError())
henryrawas 9:a0fb6c370dbb 270 {
henryrawas 9:a0fb6c370dbb 271 _lastErrorPart = ix;
henryrawas 9:a0fb6c370dbb 272 _lastError = _armParts[ix]->GetLastError();
henryrawas 9:a0fb6c370dbb 273 ok = false;
henryrawas 9:a0fb6c370dbb 274 }
henryrawas 4:36a4eceb1b7f 275 }
henryrawas 9:a0fb6c370dbb 276 return ok;
henryrawas 4:36a4eceb1b7f 277 }
henryrawas 4:36a4eceb1b7f 278
henryrawas 7:6723f6887d00 279 // get all parts measurements
henryrawas 8:d98e2dec0f40 280 bool RobotArm::GetArmMeasure(int measureId, vector<float>& outVals)
henryrawas 4:36a4eceb1b7f 281 {
henryrawas 9:a0fb6c370dbb 282 bool ok = true;
henryrawas 8:d98e2dec0f40 283 outVals.clear();
henryrawas 8:d98e2dec0f40 284 for (int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 285 {
henryrawas 8:d98e2dec0f40 286 float val = _armParts[ix]->GetMeasure(measureId);
henryrawas 8:d98e2dec0f40 287 outVals.push_back(val);
henryrawas 9:a0fb6c370dbb 288 if (_armParts[ix]->HasError())
henryrawas 9:a0fb6c370dbb 289 {
henryrawas 9:a0fb6c370dbb 290 _lastErrorPart = ix;
henryrawas 9:a0fb6c370dbb 291 _lastError = _armParts[ix]->GetLastError();
henryrawas 9:a0fb6c370dbb 292 ok = false;
henryrawas 9:a0fb6c370dbb 293 }
henryrawas 4:36a4eceb1b7f 294 }
henryrawas 9:a0fb6c370dbb 295 return ok;
henryrawas 4:36a4eceb1b7f 296 }
henryrawas 4:36a4eceb1b7f 297
henryrawas 7:6723f6887d00 298 // get all parts last measurements
henryrawas 8:d98e2dec0f40 299 bool RobotArm::GetArmLastMeasure(int measureId, vector<float>& outVals)
henryrawas 4:36a4eceb1b7f 300 {
henryrawas 9:a0fb6c370dbb 301 bool ok = true;
henryrawas 8:d98e2dec0f40 302 outVals.clear();
henryrawas 8:d98e2dec0f40 303 for (int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 304 {
henryrawas 8:d98e2dec0f40 305 float val = _armParts[ix]->GetLastMeasure(measureId);
henryrawas 8:d98e2dec0f40 306 outVals.push_back(val);
henryrawas 9:a0fb6c370dbb 307 if (_armParts[ix]->HasError())
henryrawas 9:a0fb6c370dbb 308 {
henryrawas 9:a0fb6c370dbb 309 _lastErrorPart = ix;
henryrawas 9:a0fb6c370dbb 310 _lastError = _armParts[ix]->GetLastError();
henryrawas 9:a0fb6c370dbb 311 ok = false;
henryrawas 9:a0fb6c370dbb 312 }
henryrawas 4:36a4eceb1b7f 313 }
henryrawas 9:a0fb6c370dbb 314 return ok;
henryrawas 4:36a4eceb1b7f 315 }
henryrawas 4:36a4eceb1b7f 316
henryrawas 4:36a4eceb1b7f 317 int RobotArm::GetNumParts()
henryrawas 4:36a4eceb1b7f 318 {
henryrawas 4:36a4eceb1b7f 319 return _numParts;
henryrawas 4:36a4eceb1b7f 320 }
henryrawas 4:36a4eceb1b7f 321
henryrawas 4:36a4eceb1b7f 322 void RobotArm::SetStepMs(int stepms)
henryrawas 4:36a4eceb1b7f 323 {
henryrawas 4:36a4eceb1b7f 324 if (stepms > 0 && stepms < 5000)
henryrawas 4:36a4eceb1b7f 325 _stepms = stepms;
henryrawas 4:36a4eceb1b7f 326 }
henryrawas 4:36a4eceb1b7f 327
henryrawas 4:36a4eceb1b7f 328 void RobotArm::SetThreadId(osThreadId tid)
henryrawas 4:36a4eceb1b7f 329 {
henryrawas 4:36a4eceb1b7f 330 _tid = tid;
henryrawas 4:36a4eceb1b7f 331 }
henryrawas 4:36a4eceb1b7f 332
henryrawas 4:36a4eceb1b7f 333 // get part by position
henryrawas 4:36a4eceb1b7f 334 RobotNode* RobotArm::GetArmPart(int partIx)
henryrawas 4:36a4eceb1b7f 335 {
henryrawas 4:36a4eceb1b7f 336 return _armParts[partIx];
henryrawas 4:36a4eceb1b7f 337 }
henryrawas 4:36a4eceb1b7f 338
henryrawas 7:6723f6887d00 339 int RobotArm::GetLastError()
henryrawas 4:36a4eceb1b7f 340 {
henryrawas 7:6723f6887d00 341 return _lastError;
henryrawas 7:6723f6887d00 342 }
henryrawas 7:6723f6887d00 343
henryrawas 7:6723f6887d00 344 float RobotArm::GetLastPosDiff()
henryrawas 7:6723f6887d00 345 {
henryrawas 7:6723f6887d00 346 return _lastPosDiff;
henryrawas 4:36a4eceb1b7f 347 }
henryrawas 4:36a4eceb1b7f 348
henryrawas 4:36a4eceb1b7f 349 int RobotArm::GetLastErrorPart()
henryrawas 4:36a4eceb1b7f 350 {
henryrawas 4:36a4eceb1b7f 351 return _lastErrorPart;
henryrawas 4:36a4eceb1b7f 352 }
henryrawas 7:6723f6887d00 353
henryrawas 7:6723f6887d00 354 void RobotArm::SetAllowance(float allow)
henryrawas 7:6723f6887d00 355 {
henryrawas 7:6723f6887d00 356 allowance = allow;
henryrawas 7:6723f6887d00 357 }