File content as of revision 13:ffeff9b5e513:

#include "mbed.h"
#include "NodeAX12.h"


NodeAX12::NodeAX12(DynamixelBus* pbus, ServoId ID) :
                _Servo(pbus, ID)
{
    _LastPosition= 0;
    _LastTemperature = 0;
    _LastVoltage = 0;
    _LastLoad = 0;
}


bool NodeAX12::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 NodeAX12::GetMeasure(int measureId)
{
    float val;
    switch (measureId)
    {
        case NM_Temperature:
            val = (float)_Servo.GetTemperature();
            if (val != 0.0f)
                _LastTemperature = val;
            return val;
            
        case NM_Degrees:
            val = (float)_Servo.GetPosition();
            if (val != 0.0f)
                _LastPosition = val;
            return val;
        
        case NM_Voltage:
            val = (float)_Servo.GetSupplyVoltage();
            if (val != 0.0f)
                _LastVoltage = val;
            return val;
            
        case NM_Load:
            val = (float)_Servo.GetLoad();
            if (val != 0.0f)
            {
                _LastLoad = val;
            }
            return val;

            
        default:
            return 0.0f;
    }
}

float NodeAX12::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 NodeAX12::HasAction(int actionId)
{
    switch (actionId)
    {
        case NA_Ping:
            return true;
            
        case NA_Init:
            return true;
            
        case NA_Rotate:
            return true;

        case NA_ClearError:
            return true;
            
        default:
            return false;
    }
}


bool NodeAX12::DoAction(int actionId, float actionValue)
{
    StatusCode sc;
    
    switch (actionId)
    {
        case NA_Ping:
            if (_Servo.Ping() == statusValid)
                return true;
            else
                return false;
            
        case NA_Init:
            sc = _Servo.TorqueEnable(false);
            sc = _Servo.TorqueEnable(true);
            return sc == statusValid;

        case NA_Rotate:
            sc = _Servo.SetGoal(actionValue);
            return sc == statusValid;

        case NA_ClearError:
            sc = _Servo.TorqueEnable(false);
            sc = _Servo.TorqueEnable(true);
            return sc == statusValid;

        default:
            return false;
    }
}

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

int NodeAX12::GetLastError()
{
    return _Servo.GetLastError();
}

bool NodeAX12::HasError()
{
    return _Servo.HasError();
}