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@18:224289104fc0, 2016-01-23 (annotated)

Committer:
henryrawas
Date:
Sat Jan 23 00:08:30 2016 +0000
Revision:
18:224289104fc0
Parent:
15:4bd10f531cdc
Child:
19:2f0ec9ac1238
refactor

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 <vector>
henryrawas 18:224289104fc0 5
henryrawas 18:224289104fc0 6 #include "DynamixelBus.h"
henryrawas 18:224289104fc0 7 #include "NodeAX12.h"
henryrawas 18:224289104fc0 8 #include "NodeEmul.h"
henryrawas 18:224289104fc0 9 #include "RobotArm.h"
henryrawas 4:36a4eceb1b7f 10
henryrawas 4:36a4eceb1b7f 11 using namespace std;
henryrawas 4:36a4eceb1b7f 12
henryrawas 18:224289104fc0 13 // create the bus interface for this device
henryrawas 11:3a2e6eb9fbb8 14 DynamixelBus dynbus( PTC17, PTC16, D7, D6, 500000 );
henryrawas 4:36a4eceb1b7f 15
henryrawas 4:36a4eceb1b7f 16
henryrawas 13:ffeff9b5e513 17 // default to move every 25 ms
henryrawas 13:ffeff9b5e513 18 #define StepPeriodMs 25
henryrawas 13:ffeff9b5e513 19
henryrawas 13:ffeff9b5e513 20 // Thresholds
henryrawas 13:ffeff9b5e513 21 // allow 3 degrees plus requested move diff for position error
henryrawas 13:ffeff9b5e513 22 #define PositionErrorAllow 3.0f
henryrawas 13:ffeff9b5e513 23 // time for continuous position error
henryrawas 13:ffeff9b5e513 24 #define FailMsLimit 250
henryrawas 13:ffeff9b5e513 25 // load level allowance
henryrawas 13:ffeff9b5e513 26 #define MaxLoadLimit 950.0f
henryrawas 13:ffeff9b5e513 27 // Temperature limit
henryrawas 13:ffeff9b5e513 28 #define MaxTemp 69
henryrawas 13:ffeff9b5e513 29 // Voltage limits
henryrawas 13:ffeff9b5e513 30 #define MaxVoltLimit 13
henryrawas 13:ffeff9b5e513 31 #define MinVoltLimit 10
henryrawas 13:ffeff9b5e513 32
henryrawas 18:224289104fc0 33 // should arm test for positions ?
henryrawas 18:224289104fc0 34 #define TestPositions false
henryrawas 18:224289104fc0 35
henryrawas 7:6723f6887d00 36
henryrawas 4:36a4eceb1b7f 37 RobotArm::RobotArm()
henryrawas 4:36a4eceb1b7f 38 {
henryrawas 4:36a4eceb1b7f 39 // build arm
henryrawas 13:ffeff9b5e513 40 for (int ix = 0; ix < NUMJOINTS; ix++)
henryrawas 10:9b21566a5ddb 41 {
henryrawas 18:224289104fc0 42 NodePartType nt = ArmJoints[ix].JointType;
henryrawas 18:224289104fc0 43 switch (nt)
henryrawas 18:224289104fc0 44 {
henryrawas 18:224289104fc0 45 case NT_AX12:
henryrawas 18:224289104fc0 46 _armParts[ix] = dynamic_cast<RobotNode*>(new NodeAX12(&dynbus, ArmJoints[ix].JointId));
henryrawas 18:224289104fc0 47 break;
henryrawas 18:224289104fc0 48
henryrawas 18:224289104fc0 49 case NT_Emul:
henryrawas 18:224289104fc0 50 _armParts[ix] = dynamic_cast<RobotNode*>(new NodeEmul(ArmJoints[ix].JointId));
henryrawas 18:224289104fc0 51 break;
henryrawas 18:224289104fc0 52
henryrawas 18:224289104fc0 53 default:
henryrawas 18:224289104fc0 54 printf("Error! Unknown node type defined");
henryrawas 18:224289104fc0 55 _armParts[ix] = NULL;
henryrawas 18:224289104fc0 56 break;
henryrawas 18:224289104fc0 57 }
henryrawas 18:224289104fc0 58
henryrawas 18:224289104fc0 59 _armParts[ix]->DoAction(NA_Init, 0.0f);
henryrawas 10:9b21566a5ddb 60 }
henryrawas 18:224289104fc0 61
henryrawas 13:ffeff9b5e513 62 _numParts = NUMJOINTS;
henryrawas 4:36a4eceb1b7f 63
henryrawas 18:224289104fc0 64 _numsteps = 0;
henryrawas 13:ffeff9b5e513 65 _stepms = StepPeriodMs;
henryrawas 4:36a4eceb1b7f 66 }
henryrawas 4:36a4eceb1b7f 67
henryrawas 13:ffeff9b5e513 68
henryrawas 11:3a2e6eb9fbb8 69 void RobotArm::ClearErrorState()
henryrawas 11:3a2e6eb9fbb8 70 {
henryrawas 13:ffeff9b5e513 71 for (int ix = 0; ix < _numParts; ix++)
henryrawas 11:3a2e6eb9fbb8 72 {
henryrawas 13:ffeff9b5e513 73 _armParts[ix]->DoAction(NA_ClearError, 0.0f);
henryrawas 18:224289104fc0 74 _failms[ix] = 0;
henryrawas 11:3a2e6eb9fbb8 75 }
henryrawas 11:3a2e6eb9fbb8 76 }
henryrawas 4:36a4eceb1b7f 77
henryrawas 4:36a4eceb1b7f 78 // move all parts to specified postions in ms time
henryrawas 13:ffeff9b5e513 79 bool RobotArm::MoveArmPositionsStart(float positions[], int totms)
henryrawas 4:36a4eceb1b7f 80 {
henryrawas 4:36a4eceb1b7f 81 _lastErrorPart = 0;
henryrawas 4:36a4eceb1b7f 82
henryrawas 4:36a4eceb1b7f 83 MoveArmPositionsEnd();
henryrawas 13:ffeff9b5e513 84
henryrawas 18:224289104fc0 85 GetArmPositions(_lastpos);
henryrawas 8:d98e2dec0f40 86
henryrawas 18:224289104fc0 87 _numsteps = totms / _stepms;
henryrawas 18:224289104fc0 88 if (_numsteps == 0) _numsteps = 1;
henryrawas 7:6723f6887d00 89
henryrawas 8:d98e2dec0f40 90 for (int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 91 {
henryrawas 7:6723f6887d00 92 if (positions[ix] > 0.0f)
henryrawas 7:6723f6887d00 93 {
henryrawas 18:224289104fc0 94 _endgoals[ix] = positions[ix];
henryrawas 18:224289104fc0 95 float difference = (positions[ix] - _lastpos[ix]) / (float)_numsteps;
henryrawas 18:224289104fc0 96 _differentials[ix] = difference;
henryrawas 7:6723f6887d00 97 }
henryrawas 7:6723f6887d00 98 else
henryrawas 7:6723f6887d00 99 {
henryrawas 7:6723f6887d00 100 // negative goal. Treat as don't move
henryrawas 18:224289104fc0 101 _differentials[ix] = 0.0f;
henryrawas 7:6723f6887d00 102 }
henryrawas 18:224289104fc0 103 _failms[ix] = 0;
henryrawas 4:36a4eceb1b7f 104 }
henryrawas 4:36a4eceb1b7f 105
henryrawas 18:224289104fc0 106 _curstep = 1;
henryrawas 4:36a4eceb1b7f 107
henryrawas 18:224289104fc0 108 _delayms = _stepms;
henryrawas 4:36a4eceb1b7f 109
henryrawas 18:224289104fc0 110 _elapseTimer.start();
henryrawas 18:224289104fc0 111 _expDelay = (int)_elapseTimer.read_ms() + _delayms;
henryrawas 13:ffeff9b5e513 112
henryrawas 13:ffeff9b5e513 113 return true;
henryrawas 13:ffeff9b5e513 114 }
henryrawas 13:ffeff9b5e513 115
henryrawas 13:ffeff9b5e513 116 // continue interrupted action
henryrawas 13:ffeff9b5e513 117 bool RobotArm::MoveArmPositionsResume()
henryrawas 13:ffeff9b5e513 118 {
henryrawas 18:224289104fc0 119 if (_curstep > _numsteps)
henryrawas 13:ffeff9b5e513 120 {
henryrawas 13:ffeff9b5e513 121 // no more steps
henryrawas 13:ffeff9b5e513 122 MoveArmPositionsEnd();
henryrawas 13:ffeff9b5e513 123 return true;
henryrawas 13:ffeff9b5e513 124 }
henryrawas 18:224289104fc0 125 GetArmPositions(_lastpos);
henryrawas 13:ffeff9b5e513 126
henryrawas 13:ffeff9b5e513 127 // reset numsteps to be what was remaining
henryrawas 18:224289104fc0 128 _numsteps = _numsteps - _curstep + 1;
henryrawas 13:ffeff9b5e513 129
henryrawas 13:ffeff9b5e513 130 for (int ix = 0; ix < _numParts; ix++)
henryrawas 13:ffeff9b5e513 131 {
henryrawas 18:224289104fc0 132 if (_endgoals[ix] > 0.0f)
henryrawas 13:ffeff9b5e513 133 {
henryrawas 18:224289104fc0 134 float difference = (_endgoals[ix] - _lastpos[ix]) / (float)_numsteps;
henryrawas 18:224289104fc0 135 _differentials[ix] = difference;
henryrawas 13:ffeff9b5e513 136 }
henryrawas 13:ffeff9b5e513 137 else
henryrawas 13:ffeff9b5e513 138 {
henryrawas 13:ffeff9b5e513 139 // negative goal. Treat as don't move
henryrawas 18:224289104fc0 140 _differentials[ix] = 0.0f;
henryrawas 13:ffeff9b5e513 141 }
henryrawas 18:224289104fc0 142 _failms[ix] = 0;
henryrawas 13:ffeff9b5e513 143 }
henryrawas 13:ffeff9b5e513 144
henryrawas 18:224289104fc0 145 _curstep = 1;
henryrawas 13:ffeff9b5e513 146
henryrawas 18:224289104fc0 147 _delayms = _stepms;
henryrawas 13:ffeff9b5e513 148
henryrawas 18:224289104fc0 149 _elapseTimer.start();
henryrawas 18:224289104fc0 150 _expDelay = (int)_elapseTimer.read_ms() + _delayms;
henryrawas 4:36a4eceb1b7f 151
henryrawas 4:36a4eceb1b7f 152 return true;
henryrawas 4:36a4eceb1b7f 153 }
henryrawas 4:36a4eceb1b7f 154
henryrawas 4:36a4eceb1b7f 155 bool RobotArm::MoveArmPositionsHasNext()
henryrawas 4:36a4eceb1b7f 156 {
henryrawas 18:224289104fc0 157 return (_curstep <= _numsteps);
henryrawas 4:36a4eceb1b7f 158 }
henryrawas 4:36a4eceb1b7f 159
henryrawas 7:6723f6887d00 160 bool RobotArm::MoveArmPositionsNext()
henryrawas 4:36a4eceb1b7f 161 {
henryrawas 4:36a4eceb1b7f 162 _lastErrorPart = 0;
henryrawas 4:36a4eceb1b7f 163
henryrawas 18:224289104fc0 164 if (_curstep > _numsteps)
henryrawas 4:36a4eceb1b7f 165 {
henryrawas 4:36a4eceb1b7f 166 // no more steps
henryrawas 4:36a4eceb1b7f 167 MoveArmPositionsEnd();
henryrawas 4:36a4eceb1b7f 168 return true;
henryrawas 4:36a4eceb1b7f 169 }
henryrawas 4:36a4eceb1b7f 170
henryrawas 4:36a4eceb1b7f 171 bool ok = true;
henryrawas 7:6723f6887d00 172
henryrawas 8:d98e2dec0f40 173 for (int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 174 {
henryrawas 4:36a4eceb1b7f 175 if (_armParts[ix]->HasAction(NA_Rotate))
henryrawas 4:36a4eceb1b7f 176 {
henryrawas 18:224289104fc0 177 float goal = (_curstep == _numsteps || _differentials[ix] == 0.0f) ?
henryrawas 18:224289104fc0 178 _endgoals[ix] : // last step - use actual goal
henryrawas 18:224289104fc0 179 (_lastpos[ix] + (_differentials[ix] * (float)_curstep));
henryrawas 18:224289104fc0 180 _lastgoals[ix] = goal;
henryrawas 18:224289104fc0 181 if (_differentials[ix] != 0.0f)
henryrawas 4:36a4eceb1b7f 182 {
henryrawas 7:6723f6887d00 183 bool ok = _armParts[ix]->DoAction(NA_Rotate, goal);
henryrawas 7:6723f6887d00 184 if (!ok)
henryrawas 7:6723f6887d00 185 {
henryrawas 7:6723f6887d00 186 _lastErrorPart = ix;
henryrawas 7:6723f6887d00 187 _lastError = _armParts[_lastErrorPart]->GetLastError();
henryrawas 7:6723f6887d00 188 _lastPosDiff = 0;
henryrawas 7:6723f6887d00 189 break;
henryrawas 7:6723f6887d00 190 }
henryrawas 4:36a4eceb1b7f 191 }
henryrawas 4:36a4eceb1b7f 192 }
henryrawas 4:36a4eceb1b7f 193 }
henryrawas 4:36a4eceb1b7f 194
henryrawas 4:36a4eceb1b7f 195 if (!ok)
henryrawas 4:36a4eceb1b7f 196 {
henryrawas 4:36a4eceb1b7f 197 return false;
henryrawas 4:36a4eceb1b7f 198 }
henryrawas 4:36a4eceb1b7f 199
henryrawas 18:224289104fc0 200 _curstep++;
henryrawas 18:224289104fc0 201 if (_curstep > _numsteps)
henryrawas 4:36a4eceb1b7f 202 {
henryrawas 4:36a4eceb1b7f 203 MoveArmPositionsEnd();
henryrawas 4:36a4eceb1b7f 204 }
henryrawas 7:6723f6887d00 205
henryrawas 7:6723f6887d00 206 return true;
henryrawas 7:6723f6887d00 207 }
henryrawas 7:6723f6887d00 208
henryrawas 7:6723f6887d00 209 // calculate actual delay until expDelay
henryrawas 7:6723f6887d00 210 bool RobotArm::MoveArmPositionsDelay(int& nextdelay)
henryrawas 7:6723f6887d00 211 {
henryrawas 18:224289104fc0 212 if (_curstep <= _numsteps)
henryrawas 7:6723f6887d00 213 {
henryrawas 18:224289104fc0 214 int elapsed = (int)_elapseTimer.read_ms();
henryrawas 4:36a4eceb1b7f 215
henryrawas 18:224289104fc0 216 if (elapsed <= _expDelay)
henryrawas 4:36a4eceb1b7f 217 {
henryrawas 18:224289104fc0 218 if (_expDelay - elapsed < _delayms)
henryrawas 18:224289104fc0 219 nextdelay = _expDelay - elapsed;
henryrawas 4:36a4eceb1b7f 220 else
henryrawas 18:224289104fc0 221 nextdelay = _delayms;
henryrawas 7:6723f6887d00 222 // set next expected time by adding step delay
henryrawas 18:224289104fc0 223 _expDelay += _delayms;
henryrawas 4:36a4eceb1b7f 224 }
henryrawas 4:36a4eceb1b7f 225 else
henryrawas 4:36a4eceb1b7f 226 {
henryrawas 18:224289104fc0 227 nextdelay = _delayms;
henryrawas 7:6723f6887d00 228 // set next expected time to now plus step delay
henryrawas 18:224289104fc0 229 _expDelay = elapsed + _delayms;
henryrawas 4:36a4eceb1b7f 230 }
henryrawas 7:6723f6887d00 231 }
henryrawas 7:6723f6887d00 232 else
henryrawas 7:6723f6887d00 233 {
henryrawas 18:224289104fc0 234 nextdelay = _delayms;
henryrawas 4:36a4eceb1b7f 235 }
henryrawas 4:36a4eceb1b7f 236
henryrawas 4:36a4eceb1b7f 237 return true;
henryrawas 4:36a4eceb1b7f 238 }
henryrawas 4:36a4eceb1b7f 239
henryrawas 13:ffeff9b5e513 240 // set goal to current position
henryrawas 13:ffeff9b5e513 241 // prevents jump when obstruction is removed
henryrawas 13:ffeff9b5e513 242 void RobotArm::MoveArmPositionsStop()
henryrawas 5:36916b1c5a06 243 {
henryrawas 13:ffeff9b5e513 244 float curpos[NUMJOINTS];
henryrawas 5:36916b1c5a06 245 GetArmPositions(curpos);
henryrawas 13:ffeff9b5e513 246
henryrawas 8:d98e2dec0f40 247 for (int ix = 0; ix < _numParts; ix++)
henryrawas 5:36916b1c5a06 248 {
henryrawas 13:ffeff9b5e513 249 (void)_armParts[ix]->DoAction(NA_Rotate, curpos[ix]);
henryrawas 13:ffeff9b5e513 250 }
henryrawas 13:ffeff9b5e513 251 }
henryrawas 7:6723f6887d00 252
henryrawas 4:36a4eceb1b7f 253 bool RobotArm::MoveArmPositionsEnd()
henryrawas 4:36a4eceb1b7f 254 {
henryrawas 18:224289104fc0 255 if (_numsteps > 0)
henryrawas 4:36a4eceb1b7f 256 {
henryrawas 18:224289104fc0 257 _elapseTimer.stop();
henryrawas 18:224289104fc0 258 _numsteps = 0;
henryrawas 4:36a4eceb1b7f 259 }
henryrawas 4:36a4eceb1b7f 260 return true;
henryrawas 4:36a4eceb1b7f 261 }
henryrawas 4:36a4eceb1b7f 262
henryrawas 15:4bd10f531cdc 263 // clear cache to force a read
henryrawas 15:4bd10f531cdc 264 void RobotArm::ArmMeasuresTestStart()
henryrawas 15:4bd10f531cdc 265 {
henryrawas 15:4bd10f531cdc 266 for (int ix = 0; ix < _numParts; ix++)
henryrawas 15:4bd10f531cdc 267 {
henryrawas 15:4bd10f531cdc 268 _armParts[ix]->ClearMeasureCache();
henryrawas 15:4bd10f531cdc 269 }
henryrawas 15:4bd10f531cdc 270 }
henryrawas 15:4bd10f531cdc 271
henryrawas 15:4bd10f531cdc 272 // test values without clearing cache
henryrawas 13:ffeff9b5e513 273 int RobotArm::ArmMeasuresTest(int measureId)
henryrawas 13:ffeff9b5e513 274 {
henryrawas 13:ffeff9b5e513 275 float curvals[NUMJOINTS];
henryrawas 13:ffeff9b5e513 276
henryrawas 15:4bd10f531cdc 277 if (!GetArmLastMeasure(measureId, curvals))
henryrawas 13:ffeff9b5e513 278 {
henryrawas 13:ffeff9b5e513 279 return -2;
henryrawas 13:ffeff9b5e513 280 }
henryrawas 13:ffeff9b5e513 281
henryrawas 13:ffeff9b5e513 282 int rc = 0;
henryrawas 13:ffeff9b5e513 283
henryrawas 13:ffeff9b5e513 284 switch (measureId)
henryrawas 13:ffeff9b5e513 285 {
henryrawas 13:ffeff9b5e513 286 case NM_Temperature:
henryrawas 13:ffeff9b5e513 287 for (int ix = 0; ix < _numParts; ix++)
henryrawas 13:ffeff9b5e513 288 {
henryrawas 13:ffeff9b5e513 289 float val = curvals[ix];
henryrawas 13:ffeff9b5e513 290 if (val > MaxTemp)
henryrawas 13:ffeff9b5e513 291 {
henryrawas 13:ffeff9b5e513 292 _lastErrorPart = ix;
henryrawas 13:ffeff9b5e513 293 rc = -1;
henryrawas 13:ffeff9b5e513 294 break;
henryrawas 13:ffeff9b5e513 295 }
henryrawas 13:ffeff9b5e513 296 }
henryrawas 13:ffeff9b5e513 297 break;
henryrawas 13:ffeff9b5e513 298
henryrawas 13:ffeff9b5e513 299 case NM_Degrees:
henryrawas 18:224289104fc0 300 if (TestPositions)
henryrawas 13:ffeff9b5e513 301 {
henryrawas 18:224289104fc0 302 // positions will frequently be off while arm is moving
henryrawas 18:224289104fc0 303 // logic checks if position is outside expected threshold for a period of time
henryrawas 18:224289104fc0 304 // may get false positives if goal is changed frequently
henryrawas 18:224289104fc0 305 for (int ix = 0; ix < _numParts; ix++)
henryrawas 13:ffeff9b5e513 306 {
henryrawas 18:224289104fc0 307 float val = curvals[ix];
henryrawas 18:224289104fc0 308 if (val > 0.0f)
henryrawas 13:ffeff9b5e513 309 {
henryrawas 18:224289104fc0 310 float diff = fabs(val - _lastgoals[ix]);
henryrawas 18:224289104fc0 311 if (diff > (fabs(_differentials[ix] * 2.0f) + PositionErrorAllow))
henryrawas 13:ffeff9b5e513 312 {
henryrawas 18:224289104fc0 313 int elapsed = (int)_elapseTimer.read_ms();
henryrawas 18:224289104fc0 314 if (_failms[ix] > 0)
henryrawas 13:ffeff9b5e513 315 {
henryrawas 18:224289104fc0 316 if (elapsed - _failms[ix] > FailMsLimit)
henryrawas 18:224289104fc0 317 {
henryrawas 18:224289104fc0 318 // continuous failure for time period
henryrawas 18:224289104fc0 319 // report failure
henryrawas 18:224289104fc0 320 _lastPosDiff = diff;
henryrawas 18:224289104fc0 321 _lastErrorPart = ix;
henryrawas 18:224289104fc0 322
henryrawas 18:224289104fc0 323 _failms[ix] = 0;
henryrawas 18:224289104fc0 324 rc = -1;
henryrawas 18:224289104fc0 325 }
henryrawas 18:224289104fc0 326 }
henryrawas 18:224289104fc0 327 else
henryrawas 18:224289104fc0 328 {
henryrawas 18:224289104fc0 329 // first failure after success
henryrawas 18:224289104fc0 330 // remember first fail time.
henryrawas 18:224289104fc0 331 _failms[ix] = elapsed;
henryrawas 13:ffeff9b5e513 332 }
henryrawas 13:ffeff9b5e513 333 }
henryrawas 13:ffeff9b5e513 334 else
henryrawas 13:ffeff9b5e513 335 {
henryrawas 18:224289104fc0 336 // within allowable range - clear time
henryrawas 18:224289104fc0 337 _failms[ix] = 0;
henryrawas 13:ffeff9b5e513 338 }
henryrawas 13:ffeff9b5e513 339 }
henryrawas 13:ffeff9b5e513 340 }
henryrawas 13:ffeff9b5e513 341 }
henryrawas 13:ffeff9b5e513 342 break;
henryrawas 13:ffeff9b5e513 343
henryrawas 13:ffeff9b5e513 344 case NM_Voltage:
henryrawas 13:ffeff9b5e513 345 for (int ix = 0; ix < _numParts; ix++)
henryrawas 13:ffeff9b5e513 346 {
henryrawas 13:ffeff9b5e513 347 float val = curvals[ix];
henryrawas 13:ffeff9b5e513 348 if (val > MaxVoltLimit || val < MinVoltLimit)
henryrawas 13:ffeff9b5e513 349 {
henryrawas 13:ffeff9b5e513 350 _lastErrorPart = ix;
henryrawas 13:ffeff9b5e513 351 rc = -1;
henryrawas 13:ffeff9b5e513 352 break;
henryrawas 13:ffeff9b5e513 353 }
henryrawas 13:ffeff9b5e513 354 }
henryrawas 13:ffeff9b5e513 355 break;
henryrawas 13:ffeff9b5e513 356
henryrawas 13:ffeff9b5e513 357 case NM_Load:
henryrawas 13:ffeff9b5e513 358 for (int ix = 0; ix < _numParts; ix++)
henryrawas 13:ffeff9b5e513 359 {
henryrawas 13:ffeff9b5e513 360 float val = curvals[ix];
henryrawas 13:ffeff9b5e513 361 if (val > MaxLoadLimit)
henryrawas 13:ffeff9b5e513 362 {
henryrawas 13:ffeff9b5e513 363 _lastErrorPart = ix;
henryrawas 13:ffeff9b5e513 364 rc = -1;
henryrawas 13:ffeff9b5e513 365 break;
henryrawas 13:ffeff9b5e513 366 }
henryrawas 13:ffeff9b5e513 367 }
henryrawas 13:ffeff9b5e513 368 break;
henryrawas 13:ffeff9b5e513 369
henryrawas 13:ffeff9b5e513 370
henryrawas 13:ffeff9b5e513 371 default:
henryrawas 13:ffeff9b5e513 372 break;
henryrawas 13:ffeff9b5e513 373 }
henryrawas 13:ffeff9b5e513 374
henryrawas 13:ffeff9b5e513 375 return rc;
henryrawas 13:ffeff9b5e513 376 }
henryrawas 4:36a4eceb1b7f 377
henryrawas 4:36a4eceb1b7f 378 // get all parts positions
henryrawas 13:ffeff9b5e513 379 bool RobotArm::GetArmPositions(float outPos[])
henryrawas 4:36a4eceb1b7f 380 {
henryrawas 9:a0fb6c370dbb 381 bool ok = true;
henryrawas 8:d98e2dec0f40 382 for (int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 383 {
henryrawas 15:4bd10f531cdc 384 _armParts[ix]->ClearMeasureCache();
henryrawas 4:36a4eceb1b7f 385 float pos = _armParts[ix]->GetMeasure(NM_Degrees);
henryrawas 13:ffeff9b5e513 386 outPos[ix] = pos;
henryrawas 9:a0fb6c370dbb 387 if (_armParts[ix]->HasError())
henryrawas 9:a0fb6c370dbb 388 {
henryrawas 9:a0fb6c370dbb 389 _lastErrorPart = ix;
henryrawas 9:a0fb6c370dbb 390 _lastError = _armParts[ix]->GetLastError();
henryrawas 9:a0fb6c370dbb 391 ok = false;
henryrawas 9:a0fb6c370dbb 392 }
henryrawas 4:36a4eceb1b7f 393 }
henryrawas 9:a0fb6c370dbb 394 return ok;
henryrawas 4:36a4eceb1b7f 395 }
henryrawas 4:36a4eceb1b7f 396
henryrawas 7:6723f6887d00 397 // get all parts last measured positions
henryrawas 13:ffeff9b5e513 398 bool RobotArm::GetArmLastPositions(float outPos[])
henryrawas 4:36a4eceb1b7f 399 {
henryrawas 9:a0fb6c370dbb 400 bool ok = true;
henryrawas 8:d98e2dec0f40 401 for (int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 402 {
henryrawas 15:4bd10f531cdc 403 float pos = _armParts[ix]->GetMeasure(NM_Degrees);
henryrawas 13:ffeff9b5e513 404 outPos[ix] = pos;
henryrawas 9:a0fb6c370dbb 405 if (_armParts[ix]->HasError())
henryrawas 9:a0fb6c370dbb 406 {
henryrawas 9:a0fb6c370dbb 407 _lastErrorPart = ix;
henryrawas 9:a0fb6c370dbb 408 _lastError = _armParts[ix]->GetLastError();
henryrawas 9:a0fb6c370dbb 409 ok = false;
henryrawas 9:a0fb6c370dbb 410 }
henryrawas 4:36a4eceb1b7f 411 }
henryrawas 9:a0fb6c370dbb 412 return ok;
henryrawas 4:36a4eceb1b7f 413 }
henryrawas 4:36a4eceb1b7f 414
henryrawas 7:6723f6887d00 415 // get all parts measurements
henryrawas 13:ffeff9b5e513 416 bool RobotArm::GetArmMeasure(int measureId, float outVals[])
henryrawas 4:36a4eceb1b7f 417 {
henryrawas 9:a0fb6c370dbb 418 bool ok = true;
henryrawas 8:d98e2dec0f40 419 for (int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 420 {
henryrawas 15:4bd10f531cdc 421 _armParts[ix]->ClearMeasureCache();
henryrawas 8:d98e2dec0f40 422 float val = _armParts[ix]->GetMeasure(measureId);
henryrawas 13:ffeff9b5e513 423 outVals[ix] = val;
henryrawas 9:a0fb6c370dbb 424 if (_armParts[ix]->HasError())
henryrawas 9:a0fb6c370dbb 425 {
henryrawas 9:a0fb6c370dbb 426 _lastErrorPart = ix;
henryrawas 9:a0fb6c370dbb 427 _lastError = _armParts[ix]->GetLastError();
henryrawas 9:a0fb6c370dbb 428 ok = false;
henryrawas 9:a0fb6c370dbb 429 }
henryrawas 4:36a4eceb1b7f 430 }
henryrawas 9:a0fb6c370dbb 431 return ok;
henryrawas 4:36a4eceb1b7f 432 }
henryrawas 4:36a4eceb1b7f 433
henryrawas 7:6723f6887d00 434 // get all parts last measurements
henryrawas 13:ffeff9b5e513 435 bool RobotArm::GetArmLastMeasure(int measureId, float outVals[])
henryrawas 4:36a4eceb1b7f 436 {
henryrawas 9:a0fb6c370dbb 437 bool ok = true;
henryrawas 8:d98e2dec0f40 438 for (int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 439 {
henryrawas 15:4bd10f531cdc 440 float val = _armParts[ix]->GetMeasure(measureId);
henryrawas 13:ffeff9b5e513 441 outVals[ix] = val;
henryrawas 9:a0fb6c370dbb 442 if (_armParts[ix]->HasError())
henryrawas 9:a0fb6c370dbb 443 {
henryrawas 9:a0fb6c370dbb 444 _lastErrorPart = ix;
henryrawas 9:a0fb6c370dbb 445 _lastError = _armParts[ix]->GetLastError();
henryrawas 9:a0fb6c370dbb 446 ok = false;
henryrawas 9:a0fb6c370dbb 447 }
henryrawas 4:36a4eceb1b7f 448 }
henryrawas 9:a0fb6c370dbb 449 return ok;
henryrawas 4:36a4eceb1b7f 450 }
henryrawas 4:36a4eceb1b7f 451
henryrawas 4:36a4eceb1b7f 452 int RobotArm::GetNumParts()
henryrawas 4:36a4eceb1b7f 453 {
henryrawas 4:36a4eceb1b7f 454 return _numParts;
henryrawas 4:36a4eceb1b7f 455 }
henryrawas 4:36a4eceb1b7f 456
henryrawas 4:36a4eceb1b7f 457 void RobotArm::SetStepMs(int stepms)
henryrawas 4:36a4eceb1b7f 458 {
henryrawas 4:36a4eceb1b7f 459 if (stepms > 0 && stepms < 5000)
henryrawas 4:36a4eceb1b7f 460 _stepms = stepms;
henryrawas 4:36a4eceb1b7f 461 }
henryrawas 4:36a4eceb1b7f 462
henryrawas 4:36a4eceb1b7f 463 void RobotArm::SetThreadId(osThreadId tid)
henryrawas 4:36a4eceb1b7f 464 {
henryrawas 4:36a4eceb1b7f 465 _tid = tid;
henryrawas 4:36a4eceb1b7f 466 }
henryrawas 4:36a4eceb1b7f 467
henryrawas 4:36a4eceb1b7f 468 // get part by position
henryrawas 4:36a4eceb1b7f 469 RobotNode* RobotArm::GetArmPart(int partIx)
henryrawas 4:36a4eceb1b7f 470 {
henryrawas 4:36a4eceb1b7f 471 return _armParts[partIx];
henryrawas 4:36a4eceb1b7f 472 }
henryrawas 4:36a4eceb1b7f 473
henryrawas 7:6723f6887d00 474 int RobotArm::GetLastError()
henryrawas 4:36a4eceb1b7f 475 {
henryrawas 7:6723f6887d00 476 return _lastError;
henryrawas 7:6723f6887d00 477 }
henryrawas 7:6723f6887d00 478
henryrawas 7:6723f6887d00 479 float RobotArm::GetLastPosDiff()
henryrawas 7:6723f6887d00 480 {
henryrawas 7:6723f6887d00 481 return _lastPosDiff;
henryrawas 4:36a4eceb1b7f 482 }
henryrawas 4:36a4eceb1b7f 483
henryrawas 4:36a4eceb1b7f 484 int RobotArm::GetLastErrorPart()
henryrawas 4:36a4eceb1b7f 485 {
henryrawas 4:36a4eceb1b7f 486 return _lastErrorPart;
henryrawas 4:36a4eceb1b7f 487 }
henryrawas 7:6723f6887d00 488