File content as of revision 5:36916b1c5a06:

#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 );
Terminal* RobotArmPc = NULL;


// set the id for each part in the chain, in order
int PartIds[] = { 2, 3, 4, 6, 1 };

RobotArm::RobotArm()
{
    // build arm
    int i = 0;
    RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]); 
    AX12* servo1 = new AX12( &dynbus, PartIds[i] );
    servo1->TorqueEnable(true);
    _armParts[i] = dynamic_cast<RobotNode*>(servo1);
    
    i++;
    RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]);  
    AX12* servo2 = new AX12( &dynbus, PartIds[i] );
    servo2->TorqueEnable(true);
    _armParts[i] = dynamic_cast<RobotNode*>(servo2);
    
    i++;
    RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]);  
    AX12* servo3 = new AX12( &dynbus, PartIds[i] );
    servo3->TorqueEnable(true);
    _armParts[i] = dynamic_cast<RobotNode*>(servo3);
    
    i++;
    RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]);  
    AX12* servo4 = new AX12( &dynbus, PartIds[i] );
    servo4->TorqueEnable(true);
    _armParts[i] = dynamic_cast<RobotNode*>(servo4);
    
    i++;
    RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[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
}

// move all parts to specified postions in ms time
bool RobotArm::MoveArmPositions(vector<float> positions, int ms, int steps)
{
    _lastErrorPart = 0;
    
    GetArmLastPositions(lastpos);

    for( int ix = 0; ix < _numParts; ix++)
    {
        float difference = positions[ix]- lastpos[ix];
        differentials.push_back( difference );
    }

    if (steps > 1000) steps = 1000;
    if (ms <= 0) ms = 1;
    
    delayms = ms / steps;
    expDelay = delayms;
    
    elapseTimer.start();
    bool ok = true;
    
    // move arm to new position in steps
    for( int step = 1; step <= steps; step++)
    {
        //pc.foreground(Red);
        //pc.background(Black);
        //pc.locate( 0 , 24 );
        float incr = (float)step / (float)steps;
        for( int ix = 0; ix < _numParts; ix++ )
        {
            //pc.printf( "goal[%d] = %f\r\n", servoIndex, goal); 
            if (_armParts[ix]->HasAction(NA_Rotate))
            {
                float goal = lastpos[ix] + (differentials[ix] * incr);
                bool ok = _armParts[ix]->DoAction(NA_Rotate, goal);
                if (!ok)
                {
                    _lastErrorPart = ix;
                    break;
                }
            }
        }
        if (!ok)
            break;
        // adjust delay
        int elapsed = (int)elapseTimer.read_ms();
        expDelay += delayms;
        if (elapsed > expDelay && elapsed - expDelay < delayms)
            wait_ms(elapsed - expDelay);
        else
            wait_ms(delayms);
    }
    
    elapseTimer.stop();
    return ok;
}

// move all parts to specified postions in ms time
bool RobotArm::MoveArmPositionsStart(vector<float> positions, int totms)
{
    _lastErrorPart = 0;

    MoveArmPositionsEnd();
    GetArmPositions(lastpos);

    differentials.clear();
    for( int ix = 0; ix < _numParts; ix++)
    {
        float difference = positions[ix] - lastpos[ix];
        differentials.push_back( difference );
        RobotArmPc->printf( "diff %d = %f\r\n", ix, difference);
    }

    numsteps = totms / _stepms;
    if (numsteps == 0) numsteps = 1;

    curstep = 1;
    
    delayms = _stepms;
    
    elapseTimer.start();
    expDelay = (int)elapseTimer.read_ms() + delayms;
    
    return true;
}

bool RobotArm::MoveArmPositionsHasNext()
{
    return (curstep <= numsteps);
}

bool RobotArm::MoveArmPositionsNext(int& nextdelay)
{
    _lastErrorPart = 0;

    if (curstep > numsteps)
    {
        // no more steps 
        MoveArmPositionsEnd();
        return true;
    }
    
    bool ok = true;
    float incr = (float)curstep / (float)numsteps;
    lastgoals.clear();
    for( int ix = 0; ix < _numParts; ix++ )
    {
        if (_armParts[ix]->HasAction(NA_Rotate))
        {
            float goal = lastpos[ix] + (differentials[ix] * incr);
            lastgoals.push_back(goal);
            bool ok = _armParts[ix]->DoAction(NA_Rotate, goal);
            if (!ok)
            {
                _lastErrorPart = ix;
                break;
            }
        }
    }
    
    if (!ok)
    {
        return false;
    }
    
    curstep++;
    if (curstep > numsteps)
    {
        MoveArmPositionsEnd();
    }
    else
    {    
        // adjust delay
        int elapsed = (int)elapseTimer.read_ms();
    
        if (elapsed < expDelay)
        {
            if (expDelay - elapsed < delayms)
                nextdelay = expDelay - elapsed;
            else
                nextdelay = delayms;
        }
        else
        {
            // no delay before next step
            nextdelay = 0;
        }
        expDelay += delayms;
    }
    
    return true;
}

bool RobotArm::TestArmPosition(int& partix, float& diffpos)
{
    vector<float> curpos;
    GetArmPositions(curpos);
    for( int ix = 0; ix < _numParts; ix++ )
    {
        float diff = abs(curpos[ix]  - lastgoals[ix]);
        if (diff > abs(differentials[ix]) + allowance)
        {
            diffpos = diff;
            partix = ix;
            return false;
        }
    }
    return true;
}
    
bool RobotArm::MoveArmPositionsEnd()
{
    if (numsteps > 0)
    {
        elapseTimer.stop();
        numsteps = 0;
    }
    return true;
}

// move one part to specified postion in ms time
bool RobotArm::MovePartPosition(int partIx, float position, int ms, int steps)
{
    _lastErrorPart = 0;
    if (!_armParts[partIx]->HasAction(NA_Rotate))
        return false;
        
    float lastpos = _armParts[partIx]->GetLastMeasure(NM_Degrees);

    float difference = position- lastpos;

    if (steps > 1000) steps = 1000;
    if (ms <= 0) ms = 1;
    
    int delayms = ms / steps;
    
    bool ok = true;
    
    // move arm to new position in steps
    for( int step = 1; step <= steps; step++)
    {
        //pc.foreground(Red);
        //pc.background(Black);
        //pc.locate( 0 , 24 );
        float incr = (float)step / (float)steps;

        //pc.printf( "goal[%d] = %f\r\n", servoIndex, goal); 
        float goal = lastpos + (difference * incr);
        bool ok = _armParts[partIx]->DoAction(NA_Rotate, goal);
        if (!ok)
        {
            _lastErrorPart = partIx;
            break;
        }
            
        wait_ms(delayms);
    }
    return ok;
}

// get all parts positions
bool RobotArm::GetArmPositions(vector<float>& outPos)
{
    outPos.clear();
    for( int ix = 0; ix < _numParts; ix++)
    {
        float pos = _armParts[ix]->GetMeasure(NM_Degrees);
        outPos.push_back( pos );
    }
    return true;
}

// get all parts positions
bool RobotArm::GetArmLastPositions(vector<float>& outPos)
{
    outPos.clear();
    for( int ix = 0; ix < _numParts; ix++)
    {
        float pos = _armParts[ix]->GetLastMeasure(NM_Degrees);
        outPos.push_back( pos );
    }
    return true;
}

// get one part position
float RobotArm::GetPartPosition(int partIx)
{
    return _armParts[partIx]->GetMeasure(NM_Degrees);
}


// get all parts positions
bool RobotArm::GetArmMeasure(int measureId, vector<float>& outPos)
{
    outPos.clear();
    for( int ix = 0; ix < _numParts; ix++)
    {
        float pos = _armParts[ix]->GetMeasure(measureId);
        outPos.push_back( pos );
    }
    return true;
}

// get all parts positions
bool RobotArm::GetArmLastMeasure(int measureId, vector<float>& outPos)
{
    outPos.clear();
    for( int ix = 0; ix < _numParts; ix++)
    {
        float pos = _armParts[ix]->GetLastMeasure(measureId);
        outPos.push_back( pos );
    }
    return true;
}

// get one part position
float RobotArm::GetPartMeasure(int measureId, int partIx)
{
    return _armParts[partIx]->GetMeasure(measureId);
}

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

unsigned char RobotArm::GetLastError()
{
    return _armParts[_lastErrorPart]->GetLastError();
}

int RobotArm::GetLastErrorPart()
{
    return _lastErrorPart;
}