ned3pro_end_effector_driver.hpp

Go to the documentation of this file.

/*
ned3pro_end_effector_driver.hpp
Copyright (C) 2024 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/>.
*/
 
#ifndef NED3PRO_END_EFFECTOR_DRIVER_HPP
#define NED3PRO_END_EFFECTOR_DRIVER_HPP
 
#include <memory>
#include <vector>
#include <string>
#include <iostream>
#include <sstream>
#include <cassert>
 
#include "abstract_end_effector_driver.hpp"
 
#include "ned3pro_end_effector_reg.hpp"
#include "common/model/end_effector_command_type_enum.hpp"
#include "common/model/end_effector_state.hpp"
 
using ::common::model::EEndEffectorCommandType;
 
namespace ttl_driver
{
 
    template <typename reg_type = Ned3ProEndEffectorReg>
    class Ned3ProEndEffectorDriver : public AbstractEndEffectorDriver
    {
    public:
        Ned3ProEndEffectorDriver(std::shared_ptr<dynamixel::PortHandler> portHandler,
                              std::shared_ptr<dynamixel::PacketHandler> packetHandler);
 
    public:
        // AbstractTtlDriver interface : we cannot define them globally in AbstractTtlDriver
        // as it is needed here for polymorphism (AbstractTtlDriver cannot be a template class and does not
        // have access to reg_type). So it seems like a duplicate of StepperDriver
        std::string str() const override;
 
        int checkModelNumber(uint8_t id) override;
        int readFirmwareVersion(uint8_t id, std::string &version) override;
 
        int readTemperature(uint8_t id, uint8_t &temperature) override;
        int readVoltage(uint8_t id, double &voltage) override;
        int readHwErrorStatus(uint8_t id, uint8_t &hardware_error_status) override;
 
        int syncReadFirmwareVersion(const std::vector<uint8_t> &id_list, std::vector<std::string> &firmware_list) override;
        int syncReadTemperature(const std::vector<uint8_t> &id_list, std::vector<uint8_t> &temperature_list) override;
        int syncReadVoltage(const std::vector<uint8_t> &id_list, std::vector<double> &voltage_list) override;
        int syncReadRawVoltage(const std::vector<uint8_t> &id_list, std::vector<double> &voltage_list) override;
        int syncReadHwStatus(const std::vector<uint8_t> &id_list, std::vector<std::pair<double, uint8_t>> &data_list) override;
 
        int syncReadHwErrorStatus(const std::vector<uint8_t> &id_list, std::vector<uint8_t> &hw_error_list) override;
 
    public:
        // AbstractEndEffectorDriver
 
        int readButton0Status(uint8_t id, common::model::EActionType &action) override;
        int readButton1Status(uint8_t id, common::model::EActionType &action) override;
        int readButton2Status(uint8_t id, common::model::EActionType &action) override;
        int syncReadButtonsStatus(const uint8_t &id, std::vector<common::model::EActionType> &action_list) override;
 
        int readAccelerometerXValue(uint8_t id, uint32_t &x_value) override;
        int readAccelerometerYValue(uint8_t id, uint32_t &y_value) override;
        int readAccelerometerZValue(uint8_t id, uint32_t &z_value) override;
 
        int readCollisionStatus(uint8_t id, bool &status) override;
 
        int readDigitalInput(uint8_t id, bool &in) override;
        int writeDigitalInput(uint8_t id, bool in) override;
        int writeDigitalOutput(uint8_t id, bool out) override;
 
        int writeCollisionThresh(uint8_t id, int thresh) override;
        int readCollisionThresh(uint8_t id, uint32_t &thresh);
 
        int writeCollisionThreshAlgo2(uint8_t id, int thresh) override;
        int readCollisionThreshAlgo2(uint8_t id, uint32_t &thresh);
    };
 
    // definition of methods
 
    template <typename reg_type>
    Ned3ProEndEffectorDriver<reg_type>::Ned3ProEndEffectorDriver(std::shared_ptr<dynamixel::PortHandler> portHandler,
                                                           std::shared_ptr<dynamixel::PacketHandler> packetHandler) : AbstractEndEffectorDriver(std::move(portHandler),
                                                                                                                                                std::move(packetHandler))
    {
    }
 
    //*****************************
    // AbstractTtlDriver interface
    //*****************************
 
    template <typename reg_type>
    std::string Ned3ProEndEffectorDriver<reg_type>::str() const
    {
        return common::model::HardwareTypeEnum(reg_type::motor_type).toString() + " : " + ttl_driver::AbstractEndEffectorDriver::str();
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::checkModelNumber(uint8_t id)
    {
        uint16_t model_number = 0;
        int ping_result = getModelNumber(id, model_number);
 
        if (ping_result == COMM_SUCCESS)
        {
            if (model_number && model_number != reg_type::MODEL_NUMBER)
            {
                return PING_WRONG_MODEL_NUMBER;
            }
        }
 
        return ping_result;
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::readFirmwareVersion(uint8_t id, std::string &version)
    {
        int res = COMM_RX_FAIL;
        uint32_t data{};
        res = read<typename reg_type::TYPE_FIRMWARE_VERSION>(reg_type::ADDR_FIRMWARE_VERSION, id, data);
        version = interpretFirmwareVersion(data);
        return res;
    }
 
    // ram read
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::readTemperature(uint8_t id, uint8_t &temperature)
    {
        return read<typename reg_type::TYPE_PRESENT_TEMPERATURE>(reg_type::ADDR_PRESENT_TEMPERATURE, id, temperature);
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::readVoltage(uint8_t id, double &voltage)
    {
        uint16_t voltage_mV = 0;
        int res = read<typename reg_type::TYPE_PRESENT_VOLTAGE>(reg_type::ADDR_PRESENT_VOLTAGE, id, voltage_mV);
        voltage = static_cast<double>(voltage_mV) / reg_type::VOLTAGE_CONVERSION;
        return res;
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::readHwErrorStatus(uint8_t id, uint8_t &hardware_error_status)
    {
        hardware_error_status = 0;
        return read<typename reg_type::TYPE_HW_ERROR_STATUS>(reg_type::ADDR_HW_ERROR_STATUS, id, hardware_error_status);
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::syncReadFirmwareVersion(const std::vector<uint8_t> &id_list, std::vector<std::string> &firmware_list)
    {
        int res = 0;
        firmware_list.clear();
        std::vector<uint32_t> data_list{};
        res = syncRead<typename reg_type::TYPE_FIRMWARE_VERSION>(reg_type::ADDR_FIRMWARE_VERSION, id_list, data_list);
        for (auto const &data : data_list)
            firmware_list.emplace_back(interpretFirmwareVersion(data));
        return res;
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::syncReadTemperature(const std::vector<uint8_t> &id_list, std::vector<uint8_t> &temperature_list)
    {
        return syncRead<typename reg_type::TYPE_PRESENT_TEMPERATURE>(reg_type::ADDR_PRESENT_TEMPERATURE, id_list, temperature_list);
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::syncReadVoltage(const std::vector<uint8_t> &id_list, std::vector<double> &voltage_list)
    {
        voltage_list.clear();
        std::vector<uint16_t> v_read;
        int res = syncRead<typename reg_type::TYPE_PRESENT_VOLTAGE>(reg_type::ADDR_PRESENT_VOLTAGE, id_list, v_read);
        for (auto const &v : v_read)
            voltage_list.emplace_back(static_cast<double>(v) / reg_type::VOLTAGE_CONVERSION);
        return res;
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::syncReadRawVoltage(const std::vector<uint8_t> &id_list, std::vector<double> &voltage_list)
    {
        voltage_list.clear();
        std::vector<uint16_t> v_read;
        int res = syncRead<typename reg_type::TYPE_PRESENT_VOLTAGE>(reg_type::ADDR_PRESENT_VOLTAGE, id_list, v_read);
        for (auto const &v : v_read)
            voltage_list.emplace_back(static_cast<double>(v));
        return res;
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::syncReadHwStatus(const std::vector<uint8_t> &id_list,
                                                          std::vector<std::pair<double, uint8_t>> &data_list)
    {
        data_list.clear();
 
        std::vector<std::array<uint8_t, 3>> raw_data;
        int res = syncReadConsecutiveBytes<uint8_t, 3>(reg_type::ADDR_PRESENT_VOLTAGE, id_list, raw_data);
 
        for (auto const &data : raw_data)
        {
            // Voltage is first reg, uint16
            auto voltage = static_cast<double>((static_cast<uint16_t>(data.at(1)) << 8) | data.at(0));
 
            // Temperature is second reg, uint8
            uint8_t temperature = data.at(2);
 
            data_list.emplace_back(std::make_pair(voltage, temperature));
        }
 
        return res;
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::syncReadHwErrorStatus(const std::vector<uint8_t> &id_list, std::vector<uint8_t> &hw_error_list)
    {
        return syncRead<typename reg_type::TYPE_HW_ERROR_STATUS>(reg_type::ADDR_HW_ERROR_STATUS, id_list, hw_error_list);
    }
 
    // buttons status
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::readButton0Status(uint8_t id,
                                                           common::model::EActionType &action)
    {
        uint8_t status;
        int res = read<typename reg_type::TYPE_BUTTON_STATUS>(reg_type::ADDR_STATE_BUTTON_CUSTOM, id, status);
        action = interpretActionValue(status);
        return res;
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::readButton1Status(uint8_t id, common::model::EActionType &action)
    {
        uint8_t status;
        int res = read<typename reg_type::TYPE_BUTTON_STATUS>(reg_type::ADDR_STATE_BUTTON_SAVE, id, status);
        action = interpretActionValue(status);
        return res;
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::readButton2Status(uint8_t id, common::model::EActionType &action)
    {
        uint8_t status;
        int res = read<typename reg_type::TYPE_BUTTON_STATUS>(reg_type::ADDR_STATE_BUTTON_FREEDRIVE, id, status);
        action = interpretActionValue(status);
        return res;
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::syncReadButtonsStatus(const uint8_t &id,
                                                               std::vector<common::model::EActionType> &action_list)
    {
        std::vector<std::array<typename reg_type::TYPE_BUTTON_STATUS, 3>> data_array_list;
        int res;
        res = syncReadConsecutiveBytes<typename reg_type::TYPE_BUTTON_STATUS, 3>(reg_type::ADDR_STATE_BUTTON_FREEDRIVE, {id}, data_array_list);
        if (res == COMM_SUCCESS && data_array_list.size() == 1)
        {
            for (auto data : data_array_list.at(0))
            {
                action_list.push_back(interpretActionValue(data));
            }
        }
        return res;
    }
    // accelerometers and collision
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::readAccelerometerXValue(uint8_t id, uint32_t &x_value)
    {
        return read<typename reg_type::TYPE_ACCELERO_VALUE_X>(reg_type::ADDR_ACCELERO_VALUE_X, id, x_value);
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::readAccelerometerYValue(uint8_t id, uint32_t &y_value)
    {
        return read<typename reg_type::TYPE_ACCELERO_VALUE_Y>(reg_type::ADDR_ACCELERO_VALUE_Y, id, y_value);
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::readAccelerometerZValue(uint8_t id, uint32_t &z_value)
    {
        return read<typename reg_type::TYPE_ACCELERO_VALUE_Z>(reg_type::ADDR_ACCELERO_VALUE_Z, id, z_value);
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::readCollisionStatus(uint8_t id, bool &status)
    {
        uint8_t value = 0;
        status = false;
        int res = read<typename reg_type::TYPE_COLLISION_STATUS>(reg_type::ADDR_COLLISION_STATUS, id, value);
        if (COMM_SUCCESS == res)
        {
            status = (value > 0) ? true : false;
        }
        return res;
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::readDigitalInput(uint8_t id, bool &in)
    {
        uint8_t value;
        int res = read<typename reg_type::TYPE_DIGITAL_IN>(reg_type::ADDR_DIGITAL_INPUT, id, value);
        in = (value > 0) ? true : false;
        return res;
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::writeDigitalInput(uint8_t id, bool in)
    {
        return COMM_TX_ERROR;
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::writeDigitalOutput(uint8_t id, bool out)
    {
        return write<typename reg_type::TYPE_DIGITAL_OUT>(reg_type::ADDR_DIGITAL_OUTPUT, id, static_cast<uint8_t>(out));
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::writeCollisionThresh(uint8_t id, int thresh)
    {
        return write<typename reg_type::TYPE_COLLISION_THRESHOLD_ALGO_1>(reg_type::ADDR_COLLISION_DETECT_THRESH_ALGO_1, id, static_cast<uint32_t>(thresh));
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::readCollisionThresh(uint8_t id, uint32_t &thresh)
    {
        return read<typename reg_type::TYPE_COLLISION_THRESHOLD_ALGO_1>(reg_type::ADDR_COLLISION_DETECT_THRESH_ALGO_1, id, thresh);
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::writeCollisionThreshAlgo2(uint8_t id, int thresh)
    {
        return write<typename reg_type::TYPE_COLLISION_THRESHOLD_ALGO_2>(reg_type::ADDR_COLLISION_DETECT_THRESH_ALGO_2, id, static_cast<uint32_t>(thresh));
    }
 
    template <typename reg_type>
    int Ned3ProEndEffectorDriver<reg_type>::readCollisionThreshAlgo2(uint8_t id, uint32_t &thresh)
    {
        return read<typename reg_type::TYPE_COLLISION_THRESHOLD_ALGO_2>(reg_type::ADDR_COLLISION_DETECT_THRESH_ALGO_2, id, thresh);
    }
 
} // ttl_driver
 
#endif // Ned3ProEndEffectorDriver