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