File content as of revision 33:8b9dcbf6d8ec:

// Copyright (c) Microsoft. All rights reserved.
// Licensed under the MIT license. See LICENSE file in the project root for full license information.

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


NodeAX12::NodeAX12(DynamixelBus* pbus, ServoId ID) :
                _Servo(pbus, ID)
{

}


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)
{
    switch (measureId)
    {
        case NM_Temperature:
            return (float)_Servo.GetTemperature();
            
        case NM_Degrees:
            return (float)_Servo.GetPosition();
        
        case NM_Voltage:
            return (float)_Servo.GetSupplyVoltage();
            
        case NM_Load:
            return (float)_Servo.GetLoad();
            
        default:
            return 0.0f;
    }
}

void NodeAX12::ClearMeasureCache()
{
    _Servo.ClearCache();
}

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.SetReplyDelay(100);
            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();
}