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:
- 7:6723f6887d00
- Parent:
- 5:36916b1c5a06
- Child:
- 8:d98e2dec0f40
diff -r feb0a6311594 -r 6723f6887d00 RobotArm.cpp --- a/RobotArm.cpp Mon Dec 28 17:29:12 2015 +0000 +++ b/RobotArm.cpp Tue Dec 29 23:31:28 2015 +0000 @@ -10,41 +10,42 @@ using namespace std; DynamixelBus dynbus( PTC17, PTC16, D7, D6, 1000000 ); -Terminal* RobotArmPc = NULL; // set the id for each part in the chain, in order int PartIds[] = { 2, 3, 4, 6, 1 }; +#define FailMsLimit 200 + RobotArm::RobotArm() { // build arm int i = 0; - RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]); + printf( "id[%d] = %d\r\n", i, PartIds[i]); AX12* servo1 = new AX12( &dynbus, PartIds[i] ); servo1->TorqueEnable(true); _armParts[i] = dynamic_cast<RobotNode*>(servo1); i++; - RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]); + printf( "id[%d] = %d\r\n", i, PartIds[i]); AX12* servo2 = new AX12( &dynbus, PartIds[i] ); servo2->TorqueEnable(true); _armParts[i] = dynamic_cast<RobotNode*>(servo2); i++; - RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]); + printf( "id[%d] = %d\r\n", i, PartIds[i]); AX12* servo3 = new AX12( &dynbus, PartIds[i] ); servo3->TorqueEnable(true); _armParts[i] = dynamic_cast<RobotNode*>(servo3); i++; - RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]); + printf( "id[%d] = %d\r\n", i, PartIds[i]); AX12* servo4 = new AX12( &dynbus, PartIds[i] ); servo4->TorqueEnable(true); _armParts[i] = dynamic_cast<RobotNode*>(servo4); i++; - RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]); + printf( "id[%d] = %d\r\n", i, PartIds[i]); AX12* servo6 = new AX12( &dynbus, PartIds[i] ); servo6->TorqueEnable(true); _armParts[i] = dynamic_cast<RobotNode*>(servo6); @@ -54,65 +55,10 @@ numsteps = 0; _stepms = 20; // move every 20 ms + allowance = 2.0f; // allow 2 degree fudge factor + failms = 0; } -// move all parts to specified postions in ms time -bool RobotArm::MoveArmPositions(vector<float> positions, int ms, int steps) -{ - _lastErrorPart = 0; - - GetArmLastPositions(lastpos); - - for( int ix = 0; ix < _numParts; ix++) - { - float difference = positions[ix]- lastpos[ix]; - differentials.push_back( difference ); - } - - if (steps > 1000) steps = 1000; - if (ms <= 0) ms = 1; - - delayms = ms / steps; - expDelay = delayms; - - elapseTimer.start(); - bool ok = true; - - // move arm to new position in steps - for( int step = 1; step <= steps; step++) - { - //pc.foreground(Red); - //pc.background(Black); - //pc.locate( 0 , 24 ); - float incr = (float)step / (float)steps; - for( int ix = 0; ix < _numParts; ix++ ) - { - //pc.printf( "goal[%d] = %f\r\n", servoIndex, goal); - if (_armParts[ix]->HasAction(NA_Rotate)) - { - float goal = lastpos[ix] + (differentials[ix] * incr); - bool ok = _armParts[ix]->DoAction(NA_Rotate, goal); - if (!ok) - { - _lastErrorPart = ix; - break; - } - } - } - if (!ok) - break; - // adjust delay - int elapsed = (int)elapseTimer.read_ms(); - expDelay += delayms; - if (elapsed > expDelay && elapsed - expDelay < delayms) - wait_ms(elapsed - expDelay); - else - wait_ms(delayms); - } - - elapseTimer.stop(); - return ok; -} // move all parts to specified postions in ms time bool RobotArm::MoveArmPositionsStart(vector<float> positions, int totms) @@ -121,24 +67,33 @@ MoveArmPositionsEnd(); GetArmPositions(lastpos); + lastgoals.clear(); + numsteps = totms / _stepms; + if (numsteps == 0) numsteps = 1; + differentials.clear(); for( int ix = 0; ix < _numParts; ix++) { - float difference = positions[ix] - lastpos[ix]; - differentials.push_back( difference ); - RobotArmPc->printf( "diff %d = %f\r\n", ix, difference); + if (positions[ix] > 0.0f) + { + float difference = (positions[ix] - lastpos[ix]) / (float)numsteps; + differentials.push_back( difference ); + } + else + { + // negative goal. Treat as don't move + differentials.push_back(0.0f); + } } - numsteps = totms / _stepms; - if (numsteps == 0) numsteps = 1; - curstep = 1; delayms = _stepms; elapseTimer.start(); expDelay = (int)elapseTimer.read_ms() + delayms; + failms = 0; return true; } @@ -148,7 +103,7 @@ return (curstep <= numsteps); } -bool RobotArm::MoveArmPositionsNext(int& nextdelay) +bool RobotArm::MoveArmPositionsNext() { _lastErrorPart = 0; @@ -160,19 +115,24 @@ } 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); + float goal = lastpos[ix] + (differentials[ix] * (float)curstep); lastgoals.push_back(goal); - bool ok = _armParts[ix]->DoAction(NA_Rotate, goal); - if (!ok) + if (differentials[ix] != 0.0f) { - _lastErrorPart = ix; - break; + bool ok = _armParts[ix]->DoAction(NA_Rotate, goal); + if (!ok) + { + _lastErrorPart = ix; + _lastError = _armParts[_lastErrorPart]->GetLastError(); + _lastPosDiff = 0; + break; + } } } } @@ -187,43 +147,83 @@ { MoveArmPositionsEnd(); } - else - { - // adjust delay + + return true; +} + +// calculate actual delay until expDelay +bool RobotArm::MoveArmPositionsDelay(int& nextdelay) +{ + if (curstep <= numsteps) + { int elapsed = (int)elapseTimer.read_ms(); - if (elapsed < expDelay) + if (elapsed <= expDelay) { if (expDelay - elapsed < delayms) nextdelay = expDelay - elapsed; else nextdelay = delayms; + // set next expected time by adding step delay + expDelay += delayms; } else { - // no delay before next step - nextdelay = 0; + nextdelay = delayms; + // set next expected time to now plus step delay + expDelay = elapsed + delayms; } - expDelay += delayms; + } + else + { + nextdelay = 0; } return true; } -bool RobotArm::TestArmPosition(int& partix, float& diffpos) +bool RobotArm::MoveArmPositionTest() { 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) + if (curpos[ix] > 0 && lastgoals.size() > ix) { - diffpos = diff; - partix = ix; - return false; + float diff = fabs(curpos[ix] - lastgoals[ix]); + if (diff > (fabs(differentials[ix] * 2.0f) + allowance)) + { + printf("Bad position for %d. Expect %f - got %f\r\n", ix, lastgoals[ix], curpos[ix]); + _lastPosDiff = diff; + _lastErrorPart = ix; + _lastError = 0; + + int elapsed = (int)elapseTimer.read_ms(); + if (failms > 0) + { + if (elapsed - failms > FailMsLimit) + { + // continuous failure for time period + // return false + lastgoals.clear(); + failms = 0; + return false; + } + // within time period. Do not report failure + return true; + } + else + { + // first failure after success + // remember time. Do not report failure + failms = elapsed; + return true; + } + } } } + // success + failms = 0; return true; } @@ -237,45 +237,6 @@ return true; } -// move one part to specified postion in ms time -bool RobotArm::MovePartPosition(int partIx, float position, int ms, int steps) -{ - _lastErrorPart = 0; - if (!_armParts[partIx]->HasAction(NA_Rotate)) - return false; - - float lastpos = _armParts[partIx]->GetLastMeasure(NM_Degrees); - - float difference = position- lastpos; - - if (steps > 1000) steps = 1000; - if (ms <= 0) ms = 1; - - int delayms = ms / steps; - - bool ok = true; - - // move arm to new position in steps - for( int step = 1; step <= steps; step++) - { - //pc.foreground(Red); - //pc.background(Black); - //pc.locate( 0 , 24 ); - float incr = (float)step / (float)steps; - - //pc.printf( "goal[%d] = %f\r\n", servoIndex, goal); - float goal = lastpos + (difference * incr); - bool ok = _armParts[partIx]->DoAction(NA_Rotate, goal); - if (!ok) - { - _lastErrorPart = partIx; - break; - } - - wait_ms(delayms); - } - return ok; -} // get all parts positions bool RobotArm::GetArmPositions(vector<float>& outPos) @@ -289,7 +250,7 @@ return true; } -// get all parts positions +// get all parts last measured positions bool RobotArm::GetArmLastPositions(vector<float>& outPos) { outPos.clear(); @@ -301,14 +262,7 @@ return true; } -// get one part position -float RobotArm::GetPartPosition(int partIx) -{ - return _armParts[partIx]->GetMeasure(NM_Degrees); -} - - -// get all parts positions +// get all parts measurements bool RobotArm::GetArmMeasure(int measureId, vector<float>& outPos) { outPos.clear(); @@ -320,7 +274,7 @@ return true; } -// get all parts positions +// get all parts last measurements bool RobotArm::GetArmLastMeasure(int measureId, vector<float>& outPos) { outPos.clear(); @@ -332,12 +286,6 @@ return true; } -// get one part position -float RobotArm::GetPartMeasure(int measureId, int partIx) -{ - return _armParts[partIx]->GetMeasure(measureId); -} - int RobotArm::GetNumParts() { return _numParts; @@ -360,12 +308,22 @@ return _armParts[partIx]; } -unsigned char RobotArm::GetLastError() +int RobotArm::GetLastError() { - return _armParts[_lastErrorPart]->GetLastError(); + return _lastError; +} + +float RobotArm::GetLastPosDiff() +{ + return _lastPosDiff; } int RobotArm::GetLastErrorPart() { return _lastErrorPart; } + +void RobotArm::SetAllowance(float allow) +{ + allowance = allow; +}