File content as of revision 12:f66c779ca018:

/* Copyright (c) 2012 Martin Smith, MIT License
 * Copyright (C) 2016 Schakra Inc, MIT License
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
 * and associated documentation files (the "Software"), to deal in the Software without restriction,
 * including without limitation the rights to use, copy, modify, merge, publish, distribute,
 * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all copies or
 * substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
 
#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;
}