hexapod/hexapod_hardware/hardware/hexapod_robot_hwi.cpp

#include "hexapod_robot/hexapod_robot_hwi.hpp"

#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"

#include <fcntl.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <fcntl.h>
#include <stdint.h>

#include <sys/ioctl.h>
#include <linux/i2c-dev.h>

extern "C" {
    #include <i2c/smbus.h>
}

namespace hexapod_robot {


HexapodRobotHardwareInterface::~HexapodRobotHardwareInterface() {
      on_deactivate(rclcpp_lifecycle::State());
}

hardware_interface::CallbackReturn 
HexapodRobotHardwareInterface::on_init(const hardware_interface::HardwareInfo &info) {
    if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) {
        return hardware_interface::CallbackReturn::ERROR;
    }

    RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "on_init");
    
    servos_powered_ = false;

    for (const hardware_interface::ComponentInfo &joint: info_.joints) {

        std::string i2c_address_parameter = joint.name + "_i2c_address";
        std::string channel_parameter = joint.name + "_channel";
        std::string counts_intersect_parameter = joint.name + "_counts_intersect";
        std::string counts_slope_parameter = joint.name + "_counts_slope";
        std::string counts_min_parameter = joint.name + "_counts_min";
        std::string counts_max_parameter = joint.name + "_counts_max";
        std::string startup_value_parameter = joint.name + "_startup_value";

        PWMOutput pwm_output;
        pwm_output.i2c_address      = stoi(info_.hardware_parameters[i2c_address_parameter], nullptr, 0);
        pwm_output.channel          = stoi(info_.hardware_parameters[channel_parameter], nullptr, 0);
        pwm_output.counts_intersect = stod(info_.hardware_parameters[counts_intersect_parameter]);
        pwm_output.counts_slope     = stod(info_.hardware_parameters[counts_slope_parameter]);
        pwm_output.counts_min       = stod(info_.hardware_parameters[counts_min_parameter]);
        pwm_output.counts_max       = stod(info_.hardware_parameters[counts_max_parameter]);
        joint_commands_[joint.name] = stod(info_.hardware_parameters[startup_value_parameter]);

        pca_available_[pwm_output.i2c_address] = false;
        joint_pwm_mapping_[joint.name] = pwm_output;

        RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "%s mapped to 0x%02x:%d. intersect: %.4f slope: %.4f", joint.name.c_str(), pwm_output.i2c_address, pwm_output.channel, pwm_output.counts_intersect, pwm_output.counts_slope);
    }


    i2c_available_ = i2c_open_(info_.hardware_parameters["i2c_device"]);
    if(i2c_available_) pca_initialize_();

    return hardware_interface::CallbackReturn::SUCCESS;
}

bool
HexapodRobotHardwareInterface::i2c_open_(const std::string& device) {
    i2c_fd_ = open(device.c_str(), O_RDWR);
    if (i2c_fd_ < 0) {
        RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "Cannot open %s, continuing in dummy mode.", info_.hardware_parameters["i2c_device"].c_str());
        return false;
    }
    return true;
}

void
HexapodRobotHardwareInterface::pca_initialize_() {
    for (const auto& [i2c_address, available]: pca_available_) {        

        ioctl(i2c_fd_, I2C_SLAVE, i2c_address);

        if(i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode1_, 0x00) < 0) {
            RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "Could not open i2c device %02x, marking as unavailable.", i2c_address);
            pca_available_[i2c_address] = false;
        } else {
            i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode1_,    pca_mode1_sleep_); 
            i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode2_,    pca_mode2_outdrv_);
            i2c_smbus_write_byte_data(i2c_fd_, pca_reg_prescale_, (uint8_t)pca_prescaler_value_);
            i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode1_,    pca_mode1_restart_);

            RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "PCA9685 at i2c 0x%02x initialized.", i2c_address);
            pca_available_[i2c_address] = true;
        }
    }
}

bool
HexapodRobotHardwareInterface::pca_set_pwm_(uint8_t i2c_address, uint8_t channel_number, uint16_t counts) {
    // if(i2c_address == 0x40 && channel_number == 0) {
    //    RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm(%02x, %02x, %04x)", i2c_address, channel_number, counts);
    // }
    if(channel_number>15) {
        RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm: channel %02x >15", channel_number);
        return false;
    }
    if(counts > 0x1000) {
        RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm: counts %04x >15", counts);
        return false;
    }
    if(!pca_available_[i2c_address]) {
    //    RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm: i2c_address %02x unavailable.", i2c_address);
        return false;
    }

    if(ioctl(i2c_fd_, I2C_SLAVE, i2c_address) < 0) {
        RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm: ioctl failed.");
        return false;
    }

    // adding channel_number * 4 to the base address for LED0 output given by the constants
    // yields the addresses of all PWM channels

    if(counts == 0) {
        // output always off
        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_l_  + channel_number * 4, 0x00); // ON_L
        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_h_  + channel_number * 4, 0x00); // ON_H
        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_l_ + channel_number * 4, 0x00); // OFF_L
        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_h_ + channel_number * 4, 0x10); // OFF_H == 0x10 => LED full OFF 
    } else if(counts == 0x1000) {
        // output always on
        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_l_  + channel_number * 4, 0x00); // ON_L
        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_h_  + channel_number * 4, 0x10); // ON_H  == 0x10 => LED full ON 
        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_l_ + channel_number * 4, 0x00); // OFF_L
        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_h_ + channel_number * 4, 0x00); // OFF_H
    } else {
        // duty cycle from 0 to counts
        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_h_  + channel_number * 4, 0x00); 
        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_l_  + channel_number * 4, 0x00); 
        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_l_ + channel_number * 4, counts & 0xff); // lower byte
        i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_h_ + channel_number * 4, counts >>   8); // shift upper byte into lower byte
    }
    return true;
}

hardware_interface::CallbackReturn
HexapodRobotHardwareInterface::on_activate(const rclcpp_lifecycle::State &) {
    RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "on_activate");
    servos_powered_ = true;
    return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn
HexapodRobotHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &) {
    RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "on_deactivate");
    servos_powered_ = false;
    write(rclcpp::Time(), rclcpp::Duration(std::chrono::nanoseconds(0)));
    return hardware_interface::CallbackReturn::SUCCESS;
}


std::vector<hardware_interface::StateInterface>
HexapodRobotHardwareInterface::export_state_interfaces() {
    std::vector<hardware_interface::StateInterface> state_interfaces;
    for (const hardware_interface::ComponentInfo &joint: info_.joints) {
        state_interfaces.emplace_back(hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_POSITION, &joint_commands_[joint.name]));
    }
    return state_interfaces;
}

std::vector<hardware_interface::CommandInterface>
HexapodRobotHardwareInterface::export_command_interfaces() {
    std::vector<hardware_interface::CommandInterface> command_interfaces;
    for (const hardware_interface::ComponentInfo &joint: info_.joints) {
        command_interfaces.emplace_back(hardware_interface::CommandInterface(joint.name, hardware_interface::HW_IF_POSITION, &joint_commands_[joint.name]));
    }
    return command_interfaces;
}


hardware_interface::return_type
HexapodRobotHardwareInterface::read(const rclcpp::Time &, const rclcpp::Duration &) {
    return hardware_interface::return_type::OK;
}

hardware_interface::return_type
HexapodRobotHardwareInterface::write(const rclcpp::Time &, const rclcpp::Duration &) {
    for (const hardware_interface::ComponentInfo &joint: info_.joints) {
        const PWMOutput &pwm_output = joint_pwm_mapping_[joint.name];
        if(servos_powered_) {
            double pwm_value = pwm_output.counts_intersect + joint_commands_[joint.name] * pwm_output.counts_slope;
            if(pwm_value < pwm_output.counts_min) {
                RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "write: pwm_value %.2f too low, clipped to %.2f", pwm_value, pwm_output.counts_min);
                pwm_value = pwm_output.counts_min;
            }
            if(pwm_value > pwm_output.counts_max) {
                RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "write: pwm_value %.2f too high, clipped to %.2f", pwm_value, pwm_output.counts_max);
                pwm_value = pwm_output.counts_max;
            }
            pca_set_pwm_(pwm_output.i2c_address, pwm_output.channel, (uint16_t)pwm_value);
        } else {
            pca_set_pwm_(pwm_output.i2c_address, pwm_output.channel, 0);
        }
    }    
    return hardware_interface::return_type::OK;
}

} // namespace

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(hexapod_robot::HexapodRobotHardwareInterface, hardware_interface::SystemInterface)