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@33:8b9dcbf6d8ec, 2016-02-04 (annotated)

Committer:
henryrawas
Date:
Thu Feb 04 20:10:33 2016 +0000
Revision:
33:8b9dcbf6d8ec
Parent:
19:2f0ec9ac1238 
update libs

Who changed what in which revision?

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