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-23
- Revision:
- 18:224289104fc0
- Parent:
- 15:4bd10f531cdc
- Child:
- 19:2f0ec9ac1238
File content as of revision 18:224289104fc0:
#include "mbed.h" #include "rtos.h" #include <vector> #include "DynamixelBus.h" #include "NodeAX12.h" #include "NodeEmul.h" #include "RobotArm.h" using namespace std; // create the bus interface for this device DynamixelBus dynbus( PTC17, PTC16, D7, D6, 500000 ); // default to move every 25 ms #define StepPeriodMs 25 // Thresholds // allow 3 degrees plus requested move diff for position error #define PositionErrorAllow 3.0f // time for continuous position error #define FailMsLimit 250 // load level allowance #define MaxLoadLimit 950.0f // Temperature limit #define MaxTemp 69 // Voltage limits #define MaxVoltLimit 13 #define MinVoltLimit 10 // should arm test for positions ? #define TestPositions false RobotArm::RobotArm() { // build arm for (int ix = 0; ix < NUMJOINTS; ix++) { NodePartType nt = ArmJoints[ix].JointType; switch (nt) { case NT_AX12: _armParts[ix] = dynamic_cast<RobotNode*>(new NodeAX12(&dynbus, ArmJoints[ix].JointId)); break; case NT_Emul: _armParts[ix] = dynamic_cast<RobotNode*>(new NodeEmul(ArmJoints[ix].JointId)); break; default: printf("Error! Unknown node type defined"); _armParts[ix] = NULL; break; } _armParts[ix]->DoAction(NA_Init, 0.0f); } _numParts = NUMJOINTS; _numsteps = 0; _stepms = StepPeriodMs; } void RobotArm::ClearErrorState() { for (int ix = 0; ix < _numParts; ix++) { _armParts[ix]->DoAction(NA_ClearError, 0.0f); _failms[ix] = 0; } } // move all parts to specified postions in ms time bool RobotArm::MoveArmPositionsStart(float positions[], int totms) { _lastErrorPart = 0; MoveArmPositionsEnd(); GetArmPositions(_lastpos); _numsteps = totms / _stepms; if (_numsteps == 0) _numsteps = 1; for (int ix = 0; ix < _numParts; ix++) { if (positions[ix] > 0.0f) { _endgoals[ix] = positions[ix]; float difference = (positions[ix] - _lastpos[ix]) / (float)_numsteps; _differentials[ix] = difference; } else { // negative goal. Treat as don't move _differentials[ix] = 0.0f; } _failms[ix] = 0; } _curstep = 1; _delayms = _stepms; _elapseTimer.start(); _expDelay = (int)_elapseTimer.read_ms() + _delayms; return true; } // continue interrupted action bool RobotArm::MoveArmPositionsResume() { if (_curstep > _numsteps) { // no more steps MoveArmPositionsEnd(); return true; } GetArmPositions(_lastpos); // reset numsteps to be what was remaining _numsteps = _numsteps - _curstep + 1; for (int ix = 0; ix < _numParts; ix++) { if (_endgoals[ix] > 0.0f) { float difference = (_endgoals[ix] - _lastpos[ix]) / (float)_numsteps; _differentials[ix] = difference; } else { // negative goal. Treat as don't move _differentials[ix] = 0.0f; } _failms[ix] = 0; } _curstep = 1; _delayms = _stepms; _elapseTimer.start(); _expDelay = (int)_elapseTimer.read_ms() + _delayms; 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; for (int ix = 0; ix < _numParts; ix++) { if (_armParts[ix]->HasAction(NA_Rotate)) { float goal = (_curstep == _numsteps || _differentials[ix] == 0.0f) ? _endgoals[ix] : // last step - use actual goal (_lastpos[ix] + (_differentials[ix] * (float)_curstep)); _lastgoals[ix] = 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; } // set goal to current position // prevents jump when obstruction is removed void RobotArm::MoveArmPositionsStop() { float curpos[NUMJOINTS]; GetArmPositions(curpos); for (int ix = 0; ix < _numParts; ix++) { (void)_armParts[ix]->DoAction(NA_Rotate, curpos[ix]); } } bool RobotArm::MoveArmPositionsEnd() { if (_numsteps > 0) { _elapseTimer.stop(); _numsteps = 0; } return true; } // clear cache to force a read void RobotArm::ArmMeasuresTestStart() { for (int ix = 0; ix < _numParts; ix++) { _armParts[ix]->ClearMeasureCache(); } } // test values without clearing cache int RobotArm::ArmMeasuresTest(int measureId) { float curvals[NUMJOINTS]; if (!GetArmLastMeasure(measureId, curvals)) { return -2; } int rc = 0; switch (measureId) { case NM_Temperature: for (int ix = 0; ix < _numParts; ix++) { float val = curvals[ix]; if (val > MaxTemp) { _lastErrorPart = ix; rc = -1; break; } } break; case NM_Degrees: if (TestPositions) { // positions will frequently be off while arm is moving // logic checks if position is outside expected threshold for a period of time // may get false positives if goal is changed frequently for (int ix = 0; ix < _numParts; ix++) { float val = curvals[ix]; if (val > 0.0f) { float diff = fabs(val - _lastgoals[ix]); if (diff > (fabs(_differentials[ix] * 2.0f) + PositionErrorAllow)) { int elapsed = (int)_elapseTimer.read_ms(); if (_failms[ix] > 0) { if (elapsed - _failms[ix] > FailMsLimit) { // continuous failure for time period // report failure _lastPosDiff = diff; _lastErrorPart = ix; _failms[ix] = 0; rc = -1; } } else { // first failure after success // remember first fail time. _failms[ix] = elapsed; } } else { // within allowable range - clear time _failms[ix] = 0; } } } } break; case NM_Voltage: for (int ix = 0; ix < _numParts; ix++) { float val = curvals[ix]; if (val > MaxVoltLimit || val < MinVoltLimit) { _lastErrorPart = ix; rc = -1; break; } } break; case NM_Load: for (int ix = 0; ix < _numParts; ix++) { float val = curvals[ix]; if (val > MaxLoadLimit) { _lastErrorPart = ix; rc = -1; break; } } break; default: break; } return rc; } // get all parts positions bool RobotArm::GetArmPositions(float outPos[]) { bool ok = true; for (int ix = 0; ix < _numParts; ix++) { _armParts[ix]->ClearMeasureCache(); float pos = _armParts[ix]->GetMeasure(NM_Degrees); outPos[ix] = pos; if (_armParts[ix]->HasError()) { _lastErrorPart = ix; _lastError = _armParts[ix]->GetLastError(); ok = false; } } return ok; } // get all parts last measured positions bool RobotArm::GetArmLastPositions(float outPos[]) { bool ok = true; for (int ix = 0; ix < _numParts; ix++) { float pos = _armParts[ix]->GetMeasure(NM_Degrees); outPos[ix] = pos; if (_armParts[ix]->HasError()) { _lastErrorPart = ix; _lastError = _armParts[ix]->GetLastError(); ok = false; } } return ok; } // get all parts measurements bool RobotArm::GetArmMeasure(int measureId, float outVals[]) { bool ok = true; for (int ix = 0; ix < _numParts; ix++) { _armParts[ix]->ClearMeasureCache(); float val = _armParts[ix]->GetMeasure(measureId); outVals[ix] = 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, float outVals[]) { bool ok = true; for (int ix = 0; ix < _numParts; ix++) { float val = _armParts[ix]->GetMeasure(measureId); outVals[ix] = 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; }