File content as of revision 9:a0fb6c370dbb:

#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
int PartIds[] = { 2, 3, 4, 6, 1 };

#define FailMsLimit  200

RobotArm::RobotArm()
{
    // build arm
    int i = 0;
    AX12* servo1 = new AX12(&dynbus, PartIds[i]);
    servo1->TorqueEnable(true);
    _armParts[i] = dynamic_cast<RobotNode*>(servo1);
    
    i++;
    AX12* servo2 = new AX12(&dynbus, PartIds[i]);
    servo2->TorqueEnable(true);
    _armParts[i] = dynamic_cast<RobotNode*>(servo2);
    
    i++;
    AX12* servo3 = new AX12(&dynbus, PartIds[i]);
    servo3->TorqueEnable(true);
    _armParts[i] = dynamic_cast<RobotNode*>(servo3);
    
    i++;
    AX12* servo4 = new AX12(&dynbus, PartIds[i]);
    servo4->TorqueEnable(true);
    _armParts[i] = dynamic_cast<RobotNode*>(servo4);
    
    i++;
    AX12* servo6 = new AX12(&dynbus, PartIds[i]);
    servo6->TorqueEnable(true);
    _armParts[i] = dynamic_cast<RobotNode*>(servo6);
    
    i++;
    _numParts = i;

    numsteps = 0;
    _stepms = 20; // move every 20 ms
    allowance = 2.0f;       // allow 2 degree fudge factor
    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;
}