File content as of revision 2:f2da3b1d9988:

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

#include "AX12.h"
#include "mbed.h"

AX12::AX12( DynamixelBus* pbus, ServoId ID)
{
    _pbus = pbus;
    _ID = ID;
}

/*****/

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)
{
    short goal = (short)((1024.0f * degrees) / 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)
{
    CommBuffer data;
    StatusCode s = _pbus->Read(_ID, ctMoving, 1, data); 
    if( s == statusValid )
    {
        return data[0] == 1;
    }
    else
    {
        return false;
    }
}

/*****/

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

/*****/

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

/*****/

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

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

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