File content as of revision 33:8b9dcbf6d8ec:

// Copyright (c) Microsoft. All rights reserved.
// Licensed under the MIT license. See LICENSE file in the project root for full license information.

#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;
}