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@13:ffeff9b5e513, 2016-01-15 (annotated)

Committer:
henryrawas
Date:
Fri Jan 15 22:02:46 2016 +0000
Revision:
13:ffeff9b5e513
Parent:
12:ac6c9d7f8c40
Child:
15:4bd10f531cdc
Always test status and send data

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 13:ffeff9b5e513 247 extern Timer IdleTimer;
henryrawas 13:ffeff9b5e513 248 int RobotArm::ArmMeasuresTest(int measureId)
henryrawas 13:ffeff9b5e513 249 {
henryrawas 13:ffeff9b5e513 250 float curvals[NUMJOINTS];
henryrawas 13:ffeff9b5e513 251
henryrawas 13:ffeff9b5e513 252 if (!GetArmMeasure(measureId, curvals))
henryrawas 13:ffeff9b5e513 253 {
henryrawas 13:ffeff9b5e513 254 return -2;
henryrawas 13:ffeff9b5e513 255 }
henryrawas 13:ffeff9b5e513 256
henryrawas 13:ffeff9b5e513 257 int rc = 0;
henryrawas 13:ffeff9b5e513 258
henryrawas 13:ffeff9b5e513 259 switch (measureId)
henryrawas 13:ffeff9b5e513 260 {
henryrawas 13:ffeff9b5e513 261 case NM_Temperature:
henryrawas 13:ffeff9b5e513 262 for (int ix = 0; ix < _numParts; ix++)
henryrawas 13:ffeff9b5e513 263 {
henryrawas 13:ffeff9b5e513 264 float val = curvals[ix];
henryrawas 13:ffeff9b5e513 265 if (val > MaxTemp)
henryrawas 13:ffeff9b5e513 266 {
henryrawas 13:ffeff9b5e513 267 _lastErrorPart = ix;
henryrawas 13:ffeff9b5e513 268 rc = -1;
henryrawas 13:ffeff9b5e513 269 break;
henryrawas 13:ffeff9b5e513 270 }
henryrawas 13:ffeff9b5e513 271 }
henryrawas 13:ffeff9b5e513 272 break;
henryrawas 13:ffeff9b5e513 273
henryrawas 13:ffeff9b5e513 274 case NM_Degrees:
henryrawas 13:ffeff9b5e513 275 for (int ix = 0; ix < _numParts; ix++)
henryrawas 13:ffeff9b5e513 276 {
henryrawas 13:ffeff9b5e513 277 float val = curvals[ix];
henryrawas 13:ffeff9b5e513 278 if (val > 0.0f)
henryrawas 13:ffeff9b5e513 279 {
henryrawas 13:ffeff9b5e513 280 float diff = fabs(val - lastgoals[ix]);
henryrawas 13:ffeff9b5e513 281 if (diff > (fabs(differentials[ix] * 2.0f) + PositionErrorAllow))
henryrawas 13:ffeff9b5e513 282 {
henryrawas 13:ffeff9b5e513 283 int elapsed = (int)elapseTimer.read_ms();
henryrawas 13:ffeff9b5e513 284 if (failms[ix] > 0)
henryrawas 13:ffeff9b5e513 285 {
henryrawas 13:ffeff9b5e513 286 if (elapsed - failms[ix] > FailMsLimit)
henryrawas 13:ffeff9b5e513 287 {
henryrawas 13:ffeff9b5e513 288 // continuous failure for time period
henryrawas 13:ffeff9b5e513 289 // report failure
henryrawas 13:ffeff9b5e513 290 _lastPosDiff = diff;
henryrawas 13:ffeff9b5e513 291 _lastErrorPart = ix;
henryrawas 13:ffeff9b5e513 292
henryrawas 13:ffeff9b5e513 293 failms[ix] = 0;
henryrawas 13:ffeff9b5e513 294 rc = -1;
henryrawas 13:ffeff9b5e513 295 }
henryrawas 13:ffeff9b5e513 296 }
henryrawas 13:ffeff9b5e513 297 else
henryrawas 13:ffeff9b5e513 298 {
henryrawas 13:ffeff9b5e513 299 // first failure after success
henryrawas 13:ffeff9b5e513 300 // remember first fail time.
henryrawas 13:ffeff9b5e513 301 failms[ix] = elapsed;
henryrawas 13:ffeff9b5e513 302 }
henryrawas 13:ffeff9b5e513 303 }
henryrawas 13:ffeff9b5e513 304 else
henryrawas 13:ffeff9b5e513 305 {
henryrawas 13:ffeff9b5e513 306 // within allowable range - clear time
henryrawas 13:ffeff9b5e513 307 failms[ix] = 0;
henryrawas 13:ffeff9b5e513 308 }
henryrawas 13:ffeff9b5e513 309 }
henryrawas 13:ffeff9b5e513 310 }
henryrawas 13:ffeff9b5e513 311 break;
henryrawas 13:ffeff9b5e513 312
henryrawas 13:ffeff9b5e513 313 case NM_Voltage:
henryrawas 13:ffeff9b5e513 314 for (int ix = 0; ix < _numParts; ix++)
henryrawas 13:ffeff9b5e513 315 {
henryrawas 13:ffeff9b5e513 316 float val = curvals[ix];
henryrawas 13:ffeff9b5e513 317 if (val > MaxVoltLimit || val < MinVoltLimit)
henryrawas 13:ffeff9b5e513 318 {
henryrawas 13:ffeff9b5e513 319 _lastErrorPart = ix;
henryrawas 13:ffeff9b5e513 320 rc = -1;
henryrawas 13:ffeff9b5e513 321 break;
henryrawas 13:ffeff9b5e513 322 }
henryrawas 13:ffeff9b5e513 323 }
henryrawas 13:ffeff9b5e513 324 break;
henryrawas 13:ffeff9b5e513 325
henryrawas 13:ffeff9b5e513 326 case NM_Load:
henryrawas 13:ffeff9b5e513 327 for (int ix = 0; ix < _numParts; ix++)
henryrawas 13:ffeff9b5e513 328 {
henryrawas 13:ffeff9b5e513 329 float val = curvals[ix];
henryrawas 13:ffeff9b5e513 330 if (val > MaxLoadLimit)
henryrawas 13:ffeff9b5e513 331 {
henryrawas 13:ffeff9b5e513 332 _lastErrorPart = ix;
henryrawas 13:ffeff9b5e513 333 rc = -1;
henryrawas 13:ffeff9b5e513 334 break;
henryrawas 13:ffeff9b5e513 335 }
henryrawas 13:ffeff9b5e513 336 }
henryrawas 13:ffeff9b5e513 337 break;
henryrawas 13:ffeff9b5e513 338
henryrawas 13:ffeff9b5e513 339
henryrawas 13:ffeff9b5e513 340 default:
henryrawas 13:ffeff9b5e513 341 break;
henryrawas 13:ffeff9b5e513 342 }
henryrawas 13:ffeff9b5e513 343
henryrawas 13:ffeff9b5e513 344 return rc;
henryrawas 13:ffeff9b5e513 345 }
henryrawas 4:36a4eceb1b7f 346
henryrawas 4:36a4eceb1b7f 347 // get all parts positions
henryrawas 13:ffeff9b5e513 348 bool RobotArm::GetArmPositions(float outPos[])
henryrawas 4:36a4eceb1b7f 349 {
henryrawas 9:a0fb6c370dbb 350 bool ok = true;
henryrawas 8:d98e2dec0f40 351 for (int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 352 {
henryrawas 4:36a4eceb1b7f 353 float pos = _armParts[ix]->GetMeasure(NM_Degrees);
henryrawas 13:ffeff9b5e513 354 outPos[ix] = pos;
henryrawas 9:a0fb6c370dbb 355 if (_armParts[ix]->HasError())
henryrawas 9:a0fb6c370dbb 356 {
henryrawas 9:a0fb6c370dbb 357 _lastErrorPart = ix;
henryrawas 9:a0fb6c370dbb 358 _lastError = _armParts[ix]->GetLastError();
henryrawas 9:a0fb6c370dbb 359 ok = false;
henryrawas 9:a0fb6c370dbb 360 }
henryrawas 4:36a4eceb1b7f 361 }
henryrawas 9:a0fb6c370dbb 362 return ok;
henryrawas 4:36a4eceb1b7f 363 }
henryrawas 4:36a4eceb1b7f 364
henryrawas 7:6723f6887d00 365 // get all parts last measured positions
henryrawas 13:ffeff9b5e513 366 bool RobotArm::GetArmLastPositions(float outPos[])
henryrawas 4:36a4eceb1b7f 367 {
henryrawas 9:a0fb6c370dbb 368 bool ok = true;
henryrawas 8:d98e2dec0f40 369 for (int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 370 {
henryrawas 4:36a4eceb1b7f 371 float pos = _armParts[ix]->GetLastMeasure(NM_Degrees);
henryrawas 13:ffeff9b5e513 372 outPos[ix] = pos;
henryrawas 9:a0fb6c370dbb 373 if (_armParts[ix]->HasError())
henryrawas 9:a0fb6c370dbb 374 {
henryrawas 9:a0fb6c370dbb 375 _lastErrorPart = ix;
henryrawas 9:a0fb6c370dbb 376 _lastError = _armParts[ix]->GetLastError();
henryrawas 9:a0fb6c370dbb 377 ok = false;
henryrawas 9:a0fb6c370dbb 378 }
henryrawas 4:36a4eceb1b7f 379 }
henryrawas 9:a0fb6c370dbb 380 return ok;
henryrawas 4:36a4eceb1b7f 381 }
henryrawas 4:36a4eceb1b7f 382
henryrawas 7:6723f6887d00 383 // get all parts measurements
henryrawas 13:ffeff9b5e513 384 bool RobotArm::GetArmMeasure(int measureId, float outVals[])
henryrawas 4:36a4eceb1b7f 385 {
henryrawas 9:a0fb6c370dbb 386 bool ok = true;
henryrawas 8:d98e2dec0f40 387 for (int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 388 {
henryrawas 8:d98e2dec0f40 389 float val = _armParts[ix]->GetMeasure(measureId);
henryrawas 13:ffeff9b5e513 390 outVals[ix] = val;
henryrawas 9:a0fb6c370dbb 391 if (_armParts[ix]->HasError())
henryrawas 9:a0fb6c370dbb 392 {
henryrawas 9:a0fb6c370dbb 393 _lastErrorPart = ix;
henryrawas 9:a0fb6c370dbb 394 _lastError = _armParts[ix]->GetLastError();
henryrawas 9:a0fb6c370dbb 395 ok = false;
henryrawas 9:a0fb6c370dbb 396 }
henryrawas 4:36a4eceb1b7f 397 }
henryrawas 9:a0fb6c370dbb 398 return ok;
henryrawas 4:36a4eceb1b7f 399 }
henryrawas 4:36a4eceb1b7f 400
henryrawas 7:6723f6887d00 401 // get all parts last measurements
henryrawas 13:ffeff9b5e513 402 bool RobotArm::GetArmLastMeasure(int measureId, float outVals[])
henryrawas 4:36a4eceb1b7f 403 {
henryrawas 9:a0fb6c370dbb 404 bool ok = true;
henryrawas 8:d98e2dec0f40 405 for (int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 406 {
henryrawas 8:d98e2dec0f40 407 float val = _armParts[ix]->GetLastMeasure(measureId);
henryrawas 13:ffeff9b5e513 408 outVals[ix] = val;
henryrawas 9:a0fb6c370dbb 409 if (_armParts[ix]->HasError())
henryrawas 9:a0fb6c370dbb 410 {
henryrawas 9:a0fb6c370dbb 411 _lastErrorPart = ix;
henryrawas 9:a0fb6c370dbb 412 _lastError = _armParts[ix]->GetLastError();
henryrawas 9:a0fb6c370dbb 413 ok = false;
henryrawas 9:a0fb6c370dbb 414 }
henryrawas 4:36a4eceb1b7f 415 }
henryrawas 9:a0fb6c370dbb 416 return ok;
henryrawas 4:36a4eceb1b7f 417 }
henryrawas 4:36a4eceb1b7f 418
henryrawas 4:36a4eceb1b7f 419 int RobotArm::GetNumParts()
henryrawas 4:36a4eceb1b7f 420 {
henryrawas 4:36a4eceb1b7f 421 return _numParts;
henryrawas 4:36a4eceb1b7f 422 }
henryrawas 4:36a4eceb1b7f 423
henryrawas 4:36a4eceb1b7f 424 void RobotArm::SetStepMs(int stepms)
henryrawas 4:36a4eceb1b7f 425 {
henryrawas 4:36a4eceb1b7f 426 if (stepms > 0 && stepms < 5000)
henryrawas 4:36a4eceb1b7f 427 _stepms = stepms;
henryrawas 4:36a4eceb1b7f 428 }
henryrawas 4:36a4eceb1b7f 429
henryrawas 4:36a4eceb1b7f 430 void RobotArm::SetThreadId(osThreadId tid)
henryrawas 4:36a4eceb1b7f 431 {
henryrawas 4:36a4eceb1b7f 432 _tid = tid;
henryrawas 4:36a4eceb1b7f 433 }
henryrawas 4:36a4eceb1b7f 434
henryrawas 4:36a4eceb1b7f 435 // get part by position
henryrawas 4:36a4eceb1b7f 436 RobotNode* RobotArm::GetArmPart(int partIx)
henryrawas 4:36a4eceb1b7f 437 {
henryrawas 4:36a4eceb1b7f 438 return _armParts[partIx];
henryrawas 4:36a4eceb1b7f 439 }
henryrawas 4:36a4eceb1b7f 440
henryrawas 7:6723f6887d00 441 int RobotArm::GetLastError()
henryrawas 4:36a4eceb1b7f 442 {
henryrawas 7:6723f6887d00 443 return _lastError;
henryrawas 7:6723f6887d00 444 }
henryrawas 7:6723f6887d00 445
henryrawas 7:6723f6887d00 446 float RobotArm::GetLastPosDiff()
henryrawas 7:6723f6887d00 447 {
henryrawas 7:6723f6887d00 448 return _lastPosDiff;
henryrawas 4:36a4eceb1b7f 449 }
henryrawas 4:36a4eceb1b7f 450
henryrawas 4:36a4eceb1b7f 451 int RobotArm::GetLastErrorPart()
henryrawas 4:36a4eceb1b7f 452 {
henryrawas 4:36a4eceb1b7f 453 return _lastErrorPart;
henryrawas 4:36a4eceb1b7f 454 }
henryrawas 7:6723f6887d00 455