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@4:36a4eceb1b7f, 2015-12-23 (annotated)

Committer:
henryrawas
Date:
Wed Dec 23 18:34:06 2015 +0000
Revision:
4:36a4eceb1b7f
Child:
5:36916b1c5a06
RobotArm plus publish IoTHub status

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 Terminal* RobotArmPc = NULL;
henryrawas 4:36a4eceb1b7f 14
henryrawas 4:36a4eceb1b7f 15 // move arm thread timer routine
henryrawas 4:36a4eceb1b7f 16 void NextMove(void const * tid)
henryrawas 4:36a4eceb1b7f 17 {
henryrawas 4:36a4eceb1b7f 18 osSignalSet((osThreadId)tid, AS_NextStep);
henryrawas 4:36a4eceb1b7f 19 }
henryrawas 4:36a4eceb1b7f 20
henryrawas 4:36a4eceb1b7f 21
henryrawas 4:36a4eceb1b7f 22 // set the id for each part in the chain, in order
henryrawas 4:36a4eceb1b7f 23 int PartIds[] = { 1, 2, 3, 4, 6 };
henryrawas 4:36a4eceb1b7f 24
henryrawas 4:36a4eceb1b7f 25 RobotArm::RobotArm()
henryrawas 4:36a4eceb1b7f 26 {
henryrawas 4:36a4eceb1b7f 27 // build arm
henryrawas 4:36a4eceb1b7f 28 int i = 0;
henryrawas 4:36a4eceb1b7f 29 RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]);
henryrawas 4:36a4eceb1b7f 30 AX12* servo1 = new AX12( &dynbus, PartIds[i] );
henryrawas 4:36a4eceb1b7f 31 servo1->TorqueEnable(true);
henryrawas 4:36a4eceb1b7f 32 _armParts[i] = dynamic_cast<RobotNode*>(servo1);
henryrawas 4:36a4eceb1b7f 33
henryrawas 4:36a4eceb1b7f 34 i++;
henryrawas 4:36a4eceb1b7f 35 RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]);
henryrawas 4:36a4eceb1b7f 36 AX12* servo2 = new AX12( &dynbus, PartIds[i] );
henryrawas 4:36a4eceb1b7f 37 servo2->TorqueEnable(true);
henryrawas 4:36a4eceb1b7f 38 _armParts[i] = dynamic_cast<RobotNode*>(servo2);
henryrawas 4:36a4eceb1b7f 39
henryrawas 4:36a4eceb1b7f 40 i++;
henryrawas 4:36a4eceb1b7f 41 RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]);
henryrawas 4:36a4eceb1b7f 42 AX12* servo3 = new AX12( &dynbus, PartIds[i] );
henryrawas 4:36a4eceb1b7f 43 servo3->TorqueEnable(true);
henryrawas 4:36a4eceb1b7f 44 _armParts[i] = dynamic_cast<RobotNode*>(servo3);
henryrawas 4:36a4eceb1b7f 45
henryrawas 4:36a4eceb1b7f 46 i++;
henryrawas 4:36a4eceb1b7f 47 RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]);
henryrawas 4:36a4eceb1b7f 48 AX12* servo4 = new AX12( &dynbus, PartIds[i] );
henryrawas 4:36a4eceb1b7f 49 servo4->TorqueEnable(true);
henryrawas 4:36a4eceb1b7f 50 _armParts[i] = dynamic_cast<RobotNode*>(servo4);
henryrawas 4:36a4eceb1b7f 51
henryrawas 4:36a4eceb1b7f 52 i++;
henryrawas 4:36a4eceb1b7f 53 RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]);
henryrawas 4:36a4eceb1b7f 54 AX12* servo6 = new AX12( &dynbus, PartIds[i] );
henryrawas 4:36a4eceb1b7f 55 servo6->TorqueEnable(true);
henryrawas 4:36a4eceb1b7f 56 _armParts[i] = dynamic_cast<RobotNode*>(servo6);
henryrawas 4:36a4eceb1b7f 57
henryrawas 4:36a4eceb1b7f 58 i++;
henryrawas 4:36a4eceb1b7f 59 _numParts = i;
henryrawas 4:36a4eceb1b7f 60
henryrawas 4:36a4eceb1b7f 61 numsteps = 0;
henryrawas 4:36a4eceb1b7f 62 _stepms = 20; // move every 20 ms
henryrawas 4:36a4eceb1b7f 63 }
henryrawas 4:36a4eceb1b7f 64
henryrawas 4:36a4eceb1b7f 65 // move all parts to specified postions in ms time
henryrawas 4:36a4eceb1b7f 66 bool RobotArm::MoveArmPositions(vector<float> positions, int ms, int steps)
henryrawas 4:36a4eceb1b7f 67 {
henryrawas 4:36a4eceb1b7f 68 _lastErrorPart = 0;
henryrawas 4:36a4eceb1b7f 69
henryrawas 4:36a4eceb1b7f 70 GetArmLastPositions(lastpos);
henryrawas 4:36a4eceb1b7f 71
henryrawas 4:36a4eceb1b7f 72 for( int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 73 {
henryrawas 4:36a4eceb1b7f 74 float difference = positions[ix]- lastpos[ix];
henryrawas 4:36a4eceb1b7f 75 differentials.push_back( difference );
henryrawas 4:36a4eceb1b7f 76 }
henryrawas 4:36a4eceb1b7f 77
henryrawas 4:36a4eceb1b7f 78 if (steps > 1000) steps = 1000;
henryrawas 4:36a4eceb1b7f 79 if (ms <= 0) ms = 1;
henryrawas 4:36a4eceb1b7f 80
henryrawas 4:36a4eceb1b7f 81 delayms = ms / steps;
henryrawas 4:36a4eceb1b7f 82 expDelay = delayms;
henryrawas 4:36a4eceb1b7f 83
henryrawas 4:36a4eceb1b7f 84 elapseTimer.start();
henryrawas 4:36a4eceb1b7f 85 bool ok = true;
henryrawas 4:36a4eceb1b7f 86
henryrawas 4:36a4eceb1b7f 87 // move arm to new position in steps
henryrawas 4:36a4eceb1b7f 88 for( int step = 1; step <= steps; step++)
henryrawas 4:36a4eceb1b7f 89 {
henryrawas 4:36a4eceb1b7f 90 //pc.foreground(Red);
henryrawas 4:36a4eceb1b7f 91 //pc.background(Black);
henryrawas 4:36a4eceb1b7f 92 //pc.locate( 0 , 24 );
henryrawas 4:36a4eceb1b7f 93 float incr = (float)step / (float)steps;
henryrawas 4:36a4eceb1b7f 94 for( int ix = 0; ix < _numParts; ix++ )
henryrawas 4:36a4eceb1b7f 95 {
henryrawas 4:36a4eceb1b7f 96 //pc.printf( "goal[%d] = %f\r\n", servoIndex, goal);
henryrawas 4:36a4eceb1b7f 97 if (_armParts[ix]->HasAction(NA_Rotate))
henryrawas 4:36a4eceb1b7f 98 {
henryrawas 4:36a4eceb1b7f 99 float goal = lastpos[ix] + (differentials[ix] * incr);
henryrawas 4:36a4eceb1b7f 100 bool ok = _armParts[ix]->DoAction(NA_Rotate, goal);
henryrawas 4:36a4eceb1b7f 101 if (!ok)
henryrawas 4:36a4eceb1b7f 102 {
henryrawas 4:36a4eceb1b7f 103 _lastErrorPart = ix;
henryrawas 4:36a4eceb1b7f 104 break;
henryrawas 4:36a4eceb1b7f 105 }
henryrawas 4:36a4eceb1b7f 106 }
henryrawas 4:36a4eceb1b7f 107 }
henryrawas 4:36a4eceb1b7f 108 if (!ok)
henryrawas 4:36a4eceb1b7f 109 break;
henryrawas 4:36a4eceb1b7f 110 // adjust delay
henryrawas 4:36a4eceb1b7f 111 int elapsed = (int)elapseTimer.read_ms();
henryrawas 4:36a4eceb1b7f 112 expDelay += delayms;
henryrawas 4:36a4eceb1b7f 113 if (elapsed > expDelay && elapsed - expDelay < delayms)
henryrawas 4:36a4eceb1b7f 114 wait_ms(elapsed - expDelay);
henryrawas 4:36a4eceb1b7f 115 else
henryrawas 4:36a4eceb1b7f 116 wait_ms(delayms);
henryrawas 4:36a4eceb1b7f 117 }
henryrawas 4:36a4eceb1b7f 118
henryrawas 4:36a4eceb1b7f 119 elapseTimer.stop();
henryrawas 4:36a4eceb1b7f 120 return ok;
henryrawas 4:36a4eceb1b7f 121 }
henryrawas 4:36a4eceb1b7f 122
henryrawas 4:36a4eceb1b7f 123 // move all parts to specified postions in ms time
henryrawas 4:36a4eceb1b7f 124 bool RobotArm::MoveArmPositionsStart(vector<float> positions, int totms)
henryrawas 4:36a4eceb1b7f 125 {
henryrawas 4:36a4eceb1b7f 126 _lastErrorPart = 0;
henryrawas 4:36a4eceb1b7f 127
henryrawas 4:36a4eceb1b7f 128 MoveArmPositionsEnd();
henryrawas 4:36a4eceb1b7f 129 GetArmLastPositions(lastpos);
henryrawas 4:36a4eceb1b7f 130
henryrawas 4:36a4eceb1b7f 131 differentials.clear();
henryrawas 4:36a4eceb1b7f 132 for( int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 133 {
henryrawas 4:36a4eceb1b7f 134 float difference = positions[ix] - lastpos[ix];
henryrawas 4:36a4eceb1b7f 135 differentials.push_back( difference );
henryrawas 4:36a4eceb1b7f 136 RobotArmPc->printf( "diff %d = %f\r\n", ix, difference);
henryrawas 4:36a4eceb1b7f 137 }
henryrawas 4:36a4eceb1b7f 138
henryrawas 4:36a4eceb1b7f 139 numsteps = totms / _stepms;
henryrawas 4:36a4eceb1b7f 140 if (numsteps == 0) numsteps = 1;
henryrawas 4:36a4eceb1b7f 141
henryrawas 4:36a4eceb1b7f 142 curstep = 1;
henryrawas 4:36a4eceb1b7f 143
henryrawas 4:36a4eceb1b7f 144 delayms = _stepms;
henryrawas 4:36a4eceb1b7f 145 expDelay = _stepms;
henryrawas 4:36a4eceb1b7f 146
henryrawas 4:36a4eceb1b7f 147 elapseTimer.start();
henryrawas 4:36a4eceb1b7f 148
henryrawas 4:36a4eceb1b7f 149 return true;
henryrawas 4:36a4eceb1b7f 150 }
henryrawas 4:36a4eceb1b7f 151
henryrawas 4:36a4eceb1b7f 152 bool RobotArm::MoveArmPositionsHasNext()
henryrawas 4:36a4eceb1b7f 153 {
henryrawas 4:36a4eceb1b7f 154 return (curstep <= numsteps);
henryrawas 4:36a4eceb1b7f 155 }
henryrawas 4:36a4eceb1b7f 156
henryrawas 4:36a4eceb1b7f 157 bool RobotArm::MoveArmPositionsNext(int& nextdelay)
henryrawas 4:36a4eceb1b7f 158 {
henryrawas 4:36a4eceb1b7f 159 _lastErrorPart = 0;
henryrawas 4:36a4eceb1b7f 160
henryrawas 4:36a4eceb1b7f 161 if (curstep > numsteps)
henryrawas 4:36a4eceb1b7f 162 {
henryrawas 4:36a4eceb1b7f 163 // no more steps
henryrawas 4:36a4eceb1b7f 164 MoveArmPositionsEnd();
henryrawas 4:36a4eceb1b7f 165 return true;
henryrawas 4:36a4eceb1b7f 166 }
henryrawas 4:36a4eceb1b7f 167
henryrawas 4:36a4eceb1b7f 168 bool ok = true;
henryrawas 4:36a4eceb1b7f 169 float incr = (float)curstep / (float)numsteps;
henryrawas 4:36a4eceb1b7f 170 for( int ix = 0; ix < _numParts; ix++ )
henryrawas 4:36a4eceb1b7f 171 {
henryrawas 4:36a4eceb1b7f 172 if (_armParts[ix]->HasAction(NA_Rotate))
henryrawas 4:36a4eceb1b7f 173 {
henryrawas 4:36a4eceb1b7f 174 float goal = lastpos[ix] + (differentials[ix] * incr);
henryrawas 4:36a4eceb1b7f 175 bool ok = _armParts[ix]->DoAction(NA_Rotate, goal);
henryrawas 4:36a4eceb1b7f 176 if (!ok)
henryrawas 4:36a4eceb1b7f 177 {
henryrawas 4:36a4eceb1b7f 178 _lastErrorPart = ix;
henryrawas 4:36a4eceb1b7f 179 break;
henryrawas 4:36a4eceb1b7f 180 }
henryrawas 4:36a4eceb1b7f 181 }
henryrawas 4:36a4eceb1b7f 182 }
henryrawas 4:36a4eceb1b7f 183
henryrawas 4:36a4eceb1b7f 184 if (!ok)
henryrawas 4:36a4eceb1b7f 185 {
henryrawas 4:36a4eceb1b7f 186 return false;
henryrawas 4:36a4eceb1b7f 187 }
henryrawas 4:36a4eceb1b7f 188
henryrawas 4:36a4eceb1b7f 189 curstep++;
henryrawas 4:36a4eceb1b7f 190 if (curstep > numsteps)
henryrawas 4:36a4eceb1b7f 191 {
henryrawas 4:36a4eceb1b7f 192 MoveArmPositionsEnd();
henryrawas 4:36a4eceb1b7f 193 }
henryrawas 4:36a4eceb1b7f 194 else
henryrawas 4:36a4eceb1b7f 195 {
henryrawas 4:36a4eceb1b7f 196 // adjust delay
henryrawas 4:36a4eceb1b7f 197 int elapsed = (int)elapseTimer.read_ms();
henryrawas 4:36a4eceb1b7f 198 expDelay += delayms;
henryrawas 4:36a4eceb1b7f 199
henryrawas 4:36a4eceb1b7f 200 if (elapsed > expDelay)
henryrawas 4:36a4eceb1b7f 201 {
henryrawas 4:36a4eceb1b7f 202 if (elapsed - expDelay < delayms)
henryrawas 4:36a4eceb1b7f 203 nextdelay = elapsed - expDelay;
henryrawas 4:36a4eceb1b7f 204 else
henryrawas 4:36a4eceb1b7f 205 nextdelay = delayms;
henryrawas 4:36a4eceb1b7f 206 }
henryrawas 4:36a4eceb1b7f 207 else
henryrawas 4:36a4eceb1b7f 208 {
henryrawas 4:36a4eceb1b7f 209 // no delay before next step
henryrawas 4:36a4eceb1b7f 210 nextdelay = 0;
henryrawas 4:36a4eceb1b7f 211 }
henryrawas 4:36a4eceb1b7f 212 }
henryrawas 4:36a4eceb1b7f 213
henryrawas 4:36a4eceb1b7f 214 return true;
henryrawas 4:36a4eceb1b7f 215 }
henryrawas 4:36a4eceb1b7f 216
henryrawas 4:36a4eceb1b7f 217 bool RobotArm::MoveArmPositionsEnd()
henryrawas 4:36a4eceb1b7f 218 {
henryrawas 4:36a4eceb1b7f 219 if (numsteps > 0)
henryrawas 4:36a4eceb1b7f 220 {
henryrawas 4:36a4eceb1b7f 221 elapseTimer.stop();
henryrawas 4:36a4eceb1b7f 222 numsteps = 0;
henryrawas 4:36a4eceb1b7f 223 }
henryrawas 4:36a4eceb1b7f 224 return true;
henryrawas 4:36a4eceb1b7f 225 }
henryrawas 4:36a4eceb1b7f 226
henryrawas 4:36a4eceb1b7f 227 // move one part to specified postion in ms time
henryrawas 4:36a4eceb1b7f 228 bool RobotArm::MovePartPosition(int partIx, float position, int ms, int steps)
henryrawas 4:36a4eceb1b7f 229 {
henryrawas 4:36a4eceb1b7f 230 _lastErrorPart = 0;
henryrawas 4:36a4eceb1b7f 231 if (!_armParts[partIx]->HasAction(NA_Rotate))
henryrawas 4:36a4eceb1b7f 232 return false;
henryrawas 4:36a4eceb1b7f 233
henryrawas 4:36a4eceb1b7f 234 float lastpos = _armParts[partIx]->GetLastMeasure(NM_Degrees);
henryrawas 4:36a4eceb1b7f 235
henryrawas 4:36a4eceb1b7f 236 float difference = position- lastpos;
henryrawas 4:36a4eceb1b7f 237
henryrawas 4:36a4eceb1b7f 238 if (steps > 1000) steps = 1000;
henryrawas 4:36a4eceb1b7f 239 if (ms <= 0) ms = 1;
henryrawas 4:36a4eceb1b7f 240
henryrawas 4:36a4eceb1b7f 241 int delayms = ms / steps;
henryrawas 4:36a4eceb1b7f 242
henryrawas 4:36a4eceb1b7f 243 bool ok = true;
henryrawas 4:36a4eceb1b7f 244
henryrawas 4:36a4eceb1b7f 245 // move arm to new position in steps
henryrawas 4:36a4eceb1b7f 246 for( int step = 1; step <= steps; step++)
henryrawas 4:36a4eceb1b7f 247 {
henryrawas 4:36a4eceb1b7f 248 //pc.foreground(Red);
henryrawas 4:36a4eceb1b7f 249 //pc.background(Black);
henryrawas 4:36a4eceb1b7f 250 //pc.locate( 0 , 24 );
henryrawas 4:36a4eceb1b7f 251 float incr = (float)step / (float)steps;
henryrawas 4:36a4eceb1b7f 252
henryrawas 4:36a4eceb1b7f 253 //pc.printf( "goal[%d] = %f\r\n", servoIndex, goal);
henryrawas 4:36a4eceb1b7f 254 float goal = lastpos + (difference * incr);
henryrawas 4:36a4eceb1b7f 255 bool ok = _armParts[partIx]->DoAction(NA_Rotate, goal);
henryrawas 4:36a4eceb1b7f 256 if (!ok)
henryrawas 4:36a4eceb1b7f 257 {
henryrawas 4:36a4eceb1b7f 258 _lastErrorPart = partIx;
henryrawas 4:36a4eceb1b7f 259 break;
henryrawas 4:36a4eceb1b7f 260 }
henryrawas 4:36a4eceb1b7f 261
henryrawas 4:36a4eceb1b7f 262 wait_ms(delayms);
henryrawas 4:36a4eceb1b7f 263 }
henryrawas 4:36a4eceb1b7f 264 return ok;
henryrawas 4:36a4eceb1b7f 265 }
henryrawas 4:36a4eceb1b7f 266
henryrawas 4:36a4eceb1b7f 267 // get all parts positions
henryrawas 4:36a4eceb1b7f 268 bool RobotArm::GetArmPositions(vector<float>& outPos)
henryrawas 4:36a4eceb1b7f 269 {
henryrawas 4:36a4eceb1b7f 270 outPos.clear();
henryrawas 4:36a4eceb1b7f 271 for( int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 272 {
henryrawas 4:36a4eceb1b7f 273 float pos = _armParts[ix]->GetMeasure(NM_Degrees);
henryrawas 4:36a4eceb1b7f 274 outPos.push_back( pos );
henryrawas 4:36a4eceb1b7f 275 }
henryrawas 4:36a4eceb1b7f 276 return true;
henryrawas 4:36a4eceb1b7f 277 }
henryrawas 4:36a4eceb1b7f 278
henryrawas 4:36a4eceb1b7f 279 // get all parts positions
henryrawas 4:36a4eceb1b7f 280 bool RobotArm::GetArmLastPositions(vector<float>& outPos)
henryrawas 4:36a4eceb1b7f 281 {
henryrawas 4:36a4eceb1b7f 282 outPos.clear();
henryrawas 4:36a4eceb1b7f 283 for( int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 284 {
henryrawas 4:36a4eceb1b7f 285 float pos = _armParts[ix]->GetLastMeasure(NM_Degrees);
henryrawas 4:36a4eceb1b7f 286 outPos.push_back( pos );
henryrawas 4:36a4eceb1b7f 287 }
henryrawas 4:36a4eceb1b7f 288 return true;
henryrawas 4:36a4eceb1b7f 289 }
henryrawas 4:36a4eceb1b7f 290
henryrawas 4:36a4eceb1b7f 291 // get one part position
henryrawas 4:36a4eceb1b7f 292 float RobotArm::GetPartPosition(int partIx)
henryrawas 4:36a4eceb1b7f 293 {
henryrawas 4:36a4eceb1b7f 294 return _armParts[partIx]->GetMeasure(NM_Degrees);
henryrawas 4:36a4eceb1b7f 295 }
henryrawas 4:36a4eceb1b7f 296
henryrawas 4:36a4eceb1b7f 297
henryrawas 4:36a4eceb1b7f 298 // get all parts positions
henryrawas 4:36a4eceb1b7f 299 bool RobotArm::GetArmMeasure(int measureId, vector<float>& outPos)
henryrawas 4:36a4eceb1b7f 300 {
henryrawas 4:36a4eceb1b7f 301 outPos.clear();
henryrawas 4:36a4eceb1b7f 302 for( int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 303 {
henryrawas 4:36a4eceb1b7f 304 float pos = _armParts[ix]->GetMeasure(measureId);
henryrawas 4:36a4eceb1b7f 305 outPos.push_back( pos );
henryrawas 4:36a4eceb1b7f 306 }
henryrawas 4:36a4eceb1b7f 307 return true;
henryrawas 4:36a4eceb1b7f 308 }
henryrawas 4:36a4eceb1b7f 309
henryrawas 4:36a4eceb1b7f 310 // get all parts positions
henryrawas 4:36a4eceb1b7f 311 bool RobotArm::GetArmLastMeasure(int measureId, vector<float>& outPos)
henryrawas 4:36a4eceb1b7f 312 {
henryrawas 4:36a4eceb1b7f 313 outPos.clear();
henryrawas 4:36a4eceb1b7f 314 for( int ix = 0; ix < _numParts; ix++)
henryrawas 4:36a4eceb1b7f 315 {
henryrawas 4:36a4eceb1b7f 316 float pos = _armParts[ix]->GetLastMeasure(measureId);
henryrawas 4:36a4eceb1b7f 317 outPos.push_back( pos );
henryrawas 4:36a4eceb1b7f 318 }
henryrawas 4:36a4eceb1b7f 319 return true;
henryrawas 4:36a4eceb1b7f 320 }
henryrawas 4:36a4eceb1b7f 321
henryrawas 4:36a4eceb1b7f 322 // get one part position
henryrawas 4:36a4eceb1b7f 323 float RobotArm::GetPartMeasure(int measureId, int partIx)
henryrawas 4:36a4eceb1b7f 324 {
henryrawas 4:36a4eceb1b7f 325 return _armParts[partIx]->GetMeasure(measureId);
henryrawas 4:36a4eceb1b7f 326 }
henryrawas 4:36a4eceb1b7f 327
henryrawas 4:36a4eceb1b7f 328 int RobotArm::GetNumParts()
henryrawas 4:36a4eceb1b7f 329 {
henryrawas 4:36a4eceb1b7f 330 return _numParts;
henryrawas 4:36a4eceb1b7f 331 }
henryrawas 4:36a4eceb1b7f 332
henryrawas 4:36a4eceb1b7f 333 void RobotArm::SetStepMs(int stepms)
henryrawas 4:36a4eceb1b7f 334 {
henryrawas 4:36a4eceb1b7f 335 if (stepms > 0 && stepms < 5000)
henryrawas 4:36a4eceb1b7f 336 _stepms = stepms;
henryrawas 4:36a4eceb1b7f 337 }
henryrawas 4:36a4eceb1b7f 338
henryrawas 4:36a4eceb1b7f 339 void RobotArm::SetThreadId(osThreadId tid)
henryrawas 4:36a4eceb1b7f 340 {
henryrawas 4:36a4eceb1b7f 341 _tid = tid;
henryrawas 4:36a4eceb1b7f 342 }
henryrawas 4:36a4eceb1b7f 343
henryrawas 4:36a4eceb1b7f 344 // get part by position
henryrawas 4:36a4eceb1b7f 345 RobotNode* RobotArm::GetArmPart(int partIx)
henryrawas 4:36a4eceb1b7f 346 {
henryrawas 4:36a4eceb1b7f 347 return _armParts[partIx];
henryrawas 4:36a4eceb1b7f 348 }
henryrawas 4:36a4eceb1b7f 349
henryrawas 4:36a4eceb1b7f 350 unsigned char RobotArm::GetLastError()
henryrawas 4:36a4eceb1b7f 351 {
henryrawas 4:36a4eceb1b7f 352 return _armParts[_lastErrorPart]->GetLastError();
henryrawas 4:36a4eceb1b7f 353 }
henryrawas 4:36a4eceb1b7f 354
henryrawas 4:36a4eceb1b7f 355 int RobotArm::GetLastErrorPart()
henryrawas 4:36a4eceb1b7f 356 {
henryrawas 4:36a4eceb1b7f 357 return _lastErrorPart;
henryrawas 4:36a4eceb1b7f 358 }