File content as of revision 11:8de493bd8922:

/* 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;
    _HasReadCache = false;
}

/*****/

void AX12::ClearCache()
{
    _HasReadCache = false;
}

/*****/

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

/*****/

StatusCode AX12::SetReplyDelay(int us)
{
    _LastError = statusValid;
    int val = 250;
    if (us < 500)
        val = ((us + 1) / 2);

    CommBuffer data;
    data.push_back( val & 0xff );

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

/*****/

bool AX12::IsMoving(void)
{
    if (!_HasReadCache)
    {
        _LastError = LoadReadCache();
        if (_LastError != statusValid)
        {
            return false;
        }
    }
    
    return _ReadCache[cctMoving] == 1;
}

/*****/

float AX12::GetPosition()
{
    if (!_HasReadCache)
    {
        _LastError = LoadReadCache();
        if (_LastError != statusValid)
        {
            return 0.0f;
        }
    }
    
    int16_t value = _ReadCache[cctPresentPositionL] | (_ReadCache[cctPresentPositionH] << 8);
    float degrees = 30.0f + ((float)value * 300.0f / 1024.0f);
    return degrees;
}

/*****/

int AX12::GetTemperature(void)
{
    if (!_HasReadCache)
    {
        _LastError = LoadReadCache();
        if (_LastError != statusValid)
        {
            return 0;
        }
    }
    
    return (int)_ReadCache[cctPresentTemperature];
}

/*****/

float AX12::GetSupplyVoltage(void)
{
    if (!_HasReadCache)
    {
        _LastError = LoadReadCache();
        if (_LastError != statusValid)
        {
            return 0.0f;
        }
    }

    return (float)_ReadCache[cctPresentVoltage] / 10.0f;
}

/*****/

float AX12::GetLoad(void)
{
    if (!_HasReadCache)
    {
        _LastError = LoadReadCache();
        if (_LastError != statusValid)
        {
            return 0.0f;
        }
    }

    int16_t value = _ReadCache[cctPresentLoadL] | (_ReadCache[cctPresentLoadH] << 8);
    return (float)(value & 0x03ff);
}

/*****/

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

/*****/

StatusCode AX12::LoadReadCache(void)
{
    _LastError = statusValid;
    CommBuffer data;
    _HasReadCache = false;
    
    StatusCode s = _pbus->Read(_ID, AX12ReadBlockStart, AX12ReadBlockLength, data); 
    if( s == statusValid )
    {
        for (int ix = 0; ix < AX12ReadBlockLength; ix++)
        {
            _ReadCache[ix] = data[ix];
        }
        _HasReadCache = true;
    }
    else
    {
        _LastError = s;
    }
    return s;
}