File content as of revision 10:e4c9b94b5879:

/* 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;
    _LastError = statusValid;
}

/*****/

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);
        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);
            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 )
    {
        return (int)data[0];
    }
    else
    {
        // try again one time
        s = _pbus->Read(_ID, ctPresentTemperature, 1, data); 
        if( s == statusValid )
        {
            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;
        return volts;
    }
    else
    {
        // try again one time
        s = _pbus->Read(_ID, ctPresentVoltage, 1, data); 
        if( s == statusValid )
        {
            float volts = (float)data[0] / 10.0f;
            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);
        return (float)(value & 0x03ff);
    }
    else
    {
        // try again one time
        s = _pbus->Read(_ID, ctPresentLoadL, 2, data); 
        if( s == statusValid )
        {
            int16_t value = data[0] | (data[1] << 8);
            return (float)(value & 0x03ff);
        }
        _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); 
}


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

bool AX12::HasError()
{
    return _LastError != statusValid;
}