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@5:36916b1c5a06, 2015-12-28 (annotated)

Committer:
henryrawas
Date:
Mon Dec 28 17:19:37 2015 +0000
Revision:
5:36916b1c5a06
Parent:
4:36a4eceb1b7f
Child:
7:6723f6887d00
Working Robotarm

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