demo project
Dependencies: AX-12A Dynamixel mbed iothub_client EthernetInterface NTPClient ConfigFile SDFileSystem iothub_amqp_transport mbed-rtos proton-c-mbed wolfSSL
Diff: RobotArm.cpp
- Revision:
- 5:36916b1c5a06
- Parent:
- 4:36a4eceb1b7f
- Child:
- 7:6723f6887d00
diff -r 36a4eceb1b7f -r 36916b1c5a06 RobotArm.cpp --- a/RobotArm.cpp Wed Dec 23 18:34:06 2015 +0000 +++ b/RobotArm.cpp Mon Dec 28 17:19:37 2015 +0000 @@ -12,15 +12,9 @@ DynamixelBus dynbus( PTC17, PTC16, D7, D6, 1000000 ); Terminal* RobotArmPc = NULL; -// move arm thread timer routine -void NextMove(void const * tid) -{ - osSignalSet((osThreadId)tid, AS_NextStep); -} - // set the id for each part in the chain, in order -int PartIds[] = { 1, 2, 3, 4, 6 }; +int PartIds[] = { 2, 3, 4, 6, 1 }; RobotArm::RobotArm() { @@ -126,7 +120,7 @@ _lastErrorPart = 0; MoveArmPositionsEnd(); - GetArmLastPositions(lastpos); + GetArmPositions(lastpos); differentials.clear(); for( int ix = 0; ix < _numParts; ix++) @@ -142,9 +136,9 @@ curstep = 1; delayms = _stepms; - expDelay = _stepms; elapseTimer.start(); + expDelay = (int)elapseTimer.read_ms() + delayms; return true; } @@ -167,11 +161,13 @@ bool ok = true; float incr = (float)curstep / (float)numsteps; + lastgoals.clear(); for( int ix = 0; ix < _numParts; ix++ ) { if (_armParts[ix]->HasAction(NA_Rotate)) { float goal = lastpos[ix] + (differentials[ix] * incr); + lastgoals.push_back(goal); bool ok = _armParts[ix]->DoAction(NA_Rotate, goal); if (!ok) { @@ -195,12 +191,11 @@ { // adjust delay int elapsed = (int)elapseTimer.read_ms(); - expDelay += delayms; - if (elapsed > expDelay) + if (elapsed < expDelay) { - if (elapsed - expDelay < delayms) - nextdelay = elapsed - expDelay; + if (expDelay - elapsed < delayms) + nextdelay = expDelay - elapsed; else nextdelay = delayms; } @@ -209,11 +204,29 @@ // no delay before next step nextdelay = 0; } + expDelay += delayms; } return true; } +bool RobotArm::TestArmPosition(int& partix, float& diffpos) +{ + vector<float> curpos; + GetArmPositions(curpos); + for( int ix = 0; ix < _numParts; ix++ ) + { + float diff = abs(curpos[ix] - lastgoals[ix]); + if (diff > abs(differentials[ix]) + allowance) + { + diffpos = diff; + partix = ix; + return false; + } + } + return true; +} + bool RobotArm::MoveArmPositionsEnd() { if (numsteps > 0)