abstract_ttl_driver.cpp

Go to the documentation of this file.

/*
    abstract_ttl_driver.cpp
    Copyright (C) 2020 Niryo
    All rights reserved.
    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.
    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.
    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http:// www.gnu.org/licenses/>.
*/
 
#include "ttl_driver/abstract_ttl_driver.hpp"
 
#include <memory>
#include <sstream>
#include <string>
#include <utility>
#include <vector>
 
using ::std::ostringstream;
using ::std::shared_ptr;
using ::std::string;
using ::std::vector;
 
namespace ttl_driver
{
 
AbstractTtlDriver::AbstractTtlDriver(std::shared_ptr<dynamixel::PortHandler> portHandler, std::shared_ptr<dynamixel::PacketHandler> packetHandler)
    : _dxlPortHandler(std::move(portHandler)), _dxlPacketHandler(std::move(packetHandler))
{
}
 
int AbstractTtlDriver::ping(uint8_t id)
{
    uint8_t dxl_error = 0;
 
    int result = _dxlPacketHandler->ping(_dxlPortHandler.get(), id, &dxl_error);
 
    return result;
}
 
int AbstractTtlDriver::getModelNumber(uint8_t id, uint16_t &model_number)
{
    uint8_t dxl_error = 0;
 
    int result = _dxlPacketHandler->ping(_dxlPortHandler.get(), id, &model_number, &dxl_error);
 
    return result;
}
 
int AbstractTtlDriver::scan(vector<uint8_t> &id_list) { return _dxlPacketHandler->broadcastPing(_dxlPortHandler.get(), id_list); }
 
int AbstractTtlDriver::reboot(uint8_t id)
{
    int result = -1;
    uint8_t dxl_error = 0;
 
    result = _dxlPacketHandler->reboot(_dxlPortHandler.get(), id, &dxl_error);
 
    return result;
}
 
std::string AbstractTtlDriver::str() const
{
    ostringstream ss;
 
    ss << "TTL Driver : "
       << "packet handler " << (_dxlPacketHandler ? "OK" : "Not Ok") << ","
       << "port handler " << (_dxlPortHandler ? "OK" : "Not Ok");
 
    return ss.str();
}
 
/*
 *  -----------------   Read Write operations   --------------------
 */
 
int AbstractTtlDriver::readCustom(uint16_t address, uint8_t data_len, uint8_t id, uint32_t &data)
{
    // clean output data first
    data = 0;
    int dxl_comm_result = COMM_TX_FAIL;
 
    switch (data_len)
    {
    case DXL_LEN_ONE_BYTE:
    {
        uint8_t read_data;
        dxl_comm_result = read<uint8_t>(address, id, read_data);
        data = read_data;
    }
    break;
    case DXL_LEN_TWO_BYTES:
    {
        uint16_t read_data;
        dxl_comm_result = read<uint16_t>(address, id, read_data);
        data = read_data;
    }
    break;
    case DXL_LEN_FOUR_BYTES:
    {
        uint32_t read_data;
        dxl_comm_result = read<uint32_t>(address, id, read_data);
        data = read_data;
    }
    break;
    default:
        printf("AbstractTtlDriver::read ERROR: Size param must be 1, 2 or 4 bytes\n");
        break;
    }
 
    return dxl_comm_result;
}
 
int AbstractTtlDriver::writeCustom(uint16_t address, uint8_t data_len, uint8_t id, uint32_t data)
{
    int dxl_comm_result = COMM_TX_FAIL;
    uint8_t error = 0;
 
    switch (data_len)
    {
    case DXL_LEN_ONE_BYTE:
        dxl_comm_result = write<uint8_t>(address, id, static_cast<uint8_t>(data));
        break;
    case DXL_LEN_TWO_BYTES:
        dxl_comm_result = write<uint16_t>(address, id, static_cast<uint16_t>(data));
        break;
    case DXL_LEN_FOUR_BYTES:
        dxl_comm_result = write<uint32_t>(address, id, data);
        break;
    default:
        printf("AbstractTtlDriver::write ERROR: Size param must be 1, 2 or 4 bytes\n");
        break;
    }
 
    if (error != 0)
    {
        printf("AbstractTtlDriver::write ERROR: device return error: id=%d, addr=%d, len=%d, err=0x%02x\n", id, address, data_len, error);
    }
 
    return dxl_comm_result;
}
 
}  // namespace ttl_driver