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-15
- Revision:
- 13:ffeff9b5e513
- Parent:
- 12:ac6c9d7f8c40
- Child:
- 15:4bd10f531cdc
File content as of revision 13:ffeff9b5e513:
#include "mbed.h" #include "rtos.h" #include <DynamixelBus.h> #include <NodeAX12.h> //#include <NodeEmul.h> #include <Terminal.h> #include <vector> #include <RobotArm.h> using namespace std; DynamixelBus dynbus( PTC17, PTC16, D7, D6, 500000 ); // set the id for each part in the chain, in order int PartIds[] = { 2, 3, 4, 6, 1 }; // 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 RobotArm::RobotArm() { // build arm for (int ix = 0; ix < NUMJOINTS; ix++) { NodeAX12* servo = new NodeAX12(&dynbus, PartIds[ix]); //NodeEmul* servo = new NodeEmul(PartIds[i]); servo->DoAction(NA_Init, 0.0f); _armParts[ix] = dynamic_cast<RobotNode*>(servo); } _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; } extern Timer IdleTimer; int RobotArm::ArmMeasuresTest(int measureId) { float curvals[NUMJOINTS]; if (!GetArmMeasure(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: 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++) { 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]->GetLastMeasure(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++) { 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]->GetLastMeasure(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; }