File content as of revision 7:155ecc801119:

/* mbed AX-12+ Servo Library - External hardware version */

#include "AX12.h"
#include "mbed.h"
#include <Terminal.h>


AX12::AX12( DynamixelBus* pbus, ServoId ID)
{
    _pbus = pbus;
    _ID = ID;
    _LastPosition= 0.0f;
    _LastTemperature = 0;
    _LastVoltage = 0;
    _LastError = 0;
    _LastLoad = 0;
}

/*****/

StatusCode AX12::Ping()
{
    return _pbus->Ping(_ID);
}

/*****/

StatusCode AX12::SetServoId(char newId)
{
    int offset = ctID;
    CommBuffer data;
    data.push_back( newId );
    StatusCode s = _pbus->Write(_ID, offset, data); 
    if( s == statusValid )
    {
        _ID = newId;
    }
    return s;
}

/*****/

StatusCode AX12::SetGoal(float degrees)
{
    _LastError = statusValid;
    short goal = (short)((1024.0f * (degrees - 30.0f)) / 300.0f);

    CommBuffer data;
    data.push_back( goal & 0xff );
    data.push_back( goal >> 8 );

    // write the packet, return the error code
    int offset = ctGoalPositionL;
    return _pbus->Write(_ID, offset, data); 
}

/*****/

bool AX12::IsMoving(void)
{
    _LastError = statusValid;
    CommBuffer data;
    StatusCode s = _pbus->Read(_ID, ctMoving, 1, data); 
    if( s == statusValid )
    {
        return data[0] == 1;
    }
    else
    {
        _LastError = s;
        return false;
    }
}

/*****/

float AX12::GetPosition()
{
    _LastError = statusValid;
    CommBuffer data;
    StatusCode s = _pbus->Read(_ID, ctPresentPositionL, 2, data); 
    if( s == statusValid )
    {
        int16_t value = data[0] | (data[1] << 8);
        float degrees = 30.0f + ((float)value * 300.0f / 1024.0f);
        _LastPosition = degrees;
        return degrees;
    }
    else
    {
        // try again one time
        s = _pbus->Read(_ID, ctPresentPositionL, 2, data); 
        if( s == statusValid )
        {
            int16_t value = data[0] | (data[1] << 8);
            float degrees = 30.0f + ((float)value * 300.0f / 1024.0f);
            _LastPosition = degrees;
            return degrees;
        }
        _LastError = s;
        return 0.0f;
    }
}

/*****/

int AX12::GetTemperature(void)
{
    _LastError = statusValid;
    CommBuffer data;
    StatusCode s = _pbus->Read(_ID, ctPresentTemperature, 1, data); 
    if( s == statusValid )
    {
        _LastTemperature = (int)data[0];
        return (int)data[0];
    }
    else
    {
        // try again one time
        s = _pbus->Read(_ID, ctPresentTemperature, 1, data); 
        if( s == statusValid )
        {
            _LastTemperature = (int)data[0];
            return (int)data[0];
        }

        _LastError = s;
        return 0;
    }
}

/*****/

float AX12::GetSupplyVoltage(void)
{
    _LastError = statusValid;
    CommBuffer data;
    StatusCode s = _pbus->Read(_ID, ctPresentVoltage, 1, data); 
    if( s == statusValid )
    {
        float volts = (float)data[0] / 10.0f;
        _LastVoltage = volts;
        return volts;
    }
    else
    {
        // try again one time
        s = _pbus->Read(_ID, ctPresentVoltage, 1, data); 
        if( s == statusValid )
        {
            float volts = (float)data[0] / 10.0f;
            _LastVoltage = volts;
            return volts;
        }
        _LastError = s;
        return 0.0f;
    }
}

/*****/

float AX12::GetLoad(void)
{
    _LastError = statusValid;
    CommBuffer data;
    StatusCode s = _pbus->Read(_ID, ctPresentLoadL, 2, data); 
    if( s == statusValid )
    {
        int16_t value = data[0] | (data[1] << 8);
        _LastLoad = (float)(value & 0x03ff);
        return _LastLoad;
    }
    else
    {
        // try again one time
        s = _pbus->Read(_ID, ctPresentLoadL, 2, data); 
        if( s == statusValid )
        {
            int16_t value = data[0] | (data[1] << 8);
            _LastLoad = (float)(value & 0x03ff);
            return _LastLoad;
        }
        _LastError = s;
        return 0.0f;
    }
}

StatusCode AX12::TorqueEnable(bool enable)
{
    _LastError = statusValid;
    CommBuffer data;
    data.push_back( enable ? 1 : 0 );

    int offset = ctTorqueEnable;
    return _pbus->Write(_ID, offset, data); 
}

bool AX12::HasMeasure(int measureId)
{
    switch (measureId)
    {
        case NM_Temperature:
            return true;
            
        case NM_Degrees:
            return true;
        
        case NM_Voltage:
            return true;
            
        case NM_Load:
            return true;

        default:
            return false;
    }
}

float AX12::GetMeasure(int measureId)
{
    switch (measureId)
    {
        case NM_Temperature:
            return (float)GetTemperature();
            
        case NM_Degrees:
            return GetPosition();
        
        case NM_Voltage:
            return GetSupplyVoltage();
            
        case NM_Load:
            return GetLoad();
            
        default:
            return 0.0f;
    }
}

float AX12::GetLastMeasure(int measureId)
{
    switch (measureId)
    {
        case NM_Temperature:
            return (float)_LastTemperature;
            
        case NM_Degrees:
            return _LastPosition;
        
        case NM_Voltage:
            return _LastVoltage;
            
        case NM_Load:
            return _LastLoad;

        default:
            return 0.0f;
    }
}

bool AX12::HasAction(int actionId)
{
    switch (actionId)
    {
        case NA_Rotate:
            return true;

        default:
            return false;
    }
}

// TODO: error code handling cleanup
bool AX12::DoAction(int actionId, float actionValue)
{
    _LastError = statusValid;
    switch (actionId)
    {
        case NA_Rotate:
            _LastError = SetGoal(actionValue);
            return _LastError == statusValid;

        default:
            return instructionError;
    }
}

NodePartType AX12::GetNodeType()
{
    return NT_AX12;
}

int AX12::GetLastError()
{
    return (int)_LastError;
}