demo project
Dependencies: AX-12A Dynamixel mbed iothub_client EthernetInterface NTPClient ConfigFile SDFileSystem iothub_amqp_transport mbed-rtos proton-c-mbed wolfSSL
RobotArm.cpp
- Committer:
- henryrawas
- Date:
- 2016-01-06
- Revision:
- 10:9b21566a5ddb
- Parent:
- 9:a0fb6c370dbb
- Child:
- 11:3a2e6eb9fbb8
File content as of revision 10:9b21566a5ddb:
#include "mbed.h" #include "rtos.h" #include <DynamixelBus.h> #include <AX12.h> #include <Terminal.h> #include <vector> #include <RobotArm.h> using namespace std; DynamixelBus dynbus( PTC17, PTC16, D7, D6, 1000000 ); // set the id for each part in the chain, in order #define NUMPARTS 5 int PartIds[] = { 2, 3, 4, 6, 1 }; #define FailMsLimit 200 RobotArm::RobotArm() { // build arm for (int i = 0; i < NUMPARTS; i++) { AX12* servo = new AX12(&dynbus, PartIds[i]); servo->TorqueEnable(false); servo->TorqueEnable(true); _armParts[i] = dynamic_cast<RobotNode*>(servo); } _numParts = NUMPARTS; numsteps = 0; _stepms = 20; // move every 20 ms allowance = 2.0f; // allow 2 degree diff for position error failms = 0; } // move all parts to specified postions in ms time bool RobotArm::MoveArmPositionsStart(vector<float> positions, int totms) { _lastErrorPart = 0; MoveArmPositionsEnd(); GetArmPositions(lastpos); lastgoals.clear(); endgoals.clear(); numsteps = totms / _stepms; if (numsteps == 0) numsteps = 1; differentials.clear(); for (int ix = 0; ix < _numParts; ix++) { if (positions[ix] > 0.0f) { endgoals.push_back(positions[ix]); float difference = (positions[ix] - lastpos[ix]) / (float)numsteps; differentials.push_back(difference); } else { // negative goal. Treat as don't move endgoals.push_back(lastpos[ix]); differentials.push_back(0.0f); } } curstep = 1; delayms = _stepms; elapseTimer.start(); expDelay = (int)elapseTimer.read_ms() + delayms; failms = 0; return true; } bool RobotArm::MoveArmPositionsHasNext() { return (curstep <= numsteps); } bool RobotArm::MoveArmPositionsNext() { _lastErrorPart = 0; if (curstep > numsteps) { // no more steps MoveArmPositionsEnd(); return true; } bool ok = true; lastgoals.clear(); for (int ix = 0; ix < _numParts; ix++) { if (_armParts[ix]->HasAction(NA_Rotate)) { float goal = (curstep == numsteps) ? endgoals[ix] : // last step - use actual goal (lastpos[ix] + (differentials[ix] * (float)curstep)); lastgoals.push_back(goal); if (differentials[ix] != 0.0f) { bool ok = _armParts[ix]->DoAction(NA_Rotate, goal); if (!ok) { _lastErrorPart = ix; _lastError = _armParts[_lastErrorPart]->GetLastError(); _lastPosDiff = 0; break; } } } } if (!ok) { return false; } curstep++; if (curstep > numsteps) { MoveArmPositionsEnd(); } 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 (expDelay - elapsed < delayms) nextdelay = expDelay - elapsed; else nextdelay = delayms; // set next expected time by adding step delay expDelay += delayms; } else { nextdelay = delayms; // set next expected time to now plus step delay expDelay = elapsed + delayms; } } else { nextdelay = delayms; } return true; } bool RobotArm::MoveArmPositionTest() { vector<float> curpos; GetArmPositions(curpos); for (int ix = 0; ix < _numParts; ix++) { if (curpos[ix] > 0 && lastgoals.size() > ix) { 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; } bool RobotArm::MoveArmPositionsEnd() { if (numsteps > 0) { elapseTimer.stop(); numsteps = 0; } return true; } // get all parts positions bool RobotArm::GetArmPositions(vector<float>& outPos) { bool ok = true; outPos.clear(); for (int ix = 0; ix < _numParts; ix++) { float pos = _armParts[ix]->GetMeasure(NM_Degrees); outPos.push_back(pos); if (_armParts[ix]->HasError()) { _lastErrorPart = ix; _lastError = _armParts[ix]->GetLastError(); ok = false; } } return ok; } // get all parts last measured positions bool RobotArm::GetArmLastPositions(vector<float>& outPos) { bool ok = true; outPos.clear(); for (int ix = 0; ix < _numParts; ix++) { float pos = _armParts[ix]->GetLastMeasure(NM_Degrees); outPos.push_back(pos); if (_armParts[ix]->HasError()) { _lastErrorPart = ix; _lastError = _armParts[ix]->GetLastError(); ok = false; } } return ok; } // get all parts measurements bool RobotArm::GetArmMeasure(int measureId, vector<float>& outVals) { bool ok = true; outVals.clear(); for (int ix = 0; ix < _numParts; ix++) { float val = _armParts[ix]->GetMeasure(measureId); outVals.push_back(val); if (_armParts[ix]->HasError()) { _lastErrorPart = ix; _lastError = _armParts[ix]->GetLastError(); ok = false; } } return ok; } // get all parts last measurements bool RobotArm::GetArmLastMeasure(int measureId, vector<float>& outVals) { bool ok = true; outVals.clear(); for (int ix = 0; ix < _numParts; ix++) { float val = _armParts[ix]->GetLastMeasure(measureId); outVals.push_back(val); if (_armParts[ix]->HasError()) { _lastErrorPart = ix; _lastError = _armParts[ix]->GetLastError(); ok = false; } } return ok; } int RobotArm::GetNumParts() { return _numParts; } void RobotArm::SetStepMs(int stepms) { if (stepms > 0 && stepms < 5000) _stepms = stepms; } void RobotArm::SetThreadId(osThreadId tid) { _tid = tid; } // get part by position RobotNode* RobotArm::GetArmPart(int partIx) { return _armParts[partIx]; } int RobotArm::GetLastError() { return _lastError; } float RobotArm::GetLastPosDiff() { return _lastPosDiff; } int RobotArm::GetLastErrorPart() { return _lastErrorPart; } void RobotArm::SetAllowance(float allow) { allowance = allow; }