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@15:4bd10f531cdc, 2016-01-19 (annotated)

Committer:
henryrawas
Date:
Tue Jan 19 19:50:07 2016 +0000
Revision:
15:4bd10f531cdc
Parent:
13:ffeff9b5e513
Child:
18:224289104fc0
read all values at once and cache. Other cleanups

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