wuselfuzz
/
hexapod_robot
Watch 1
Star 0
Fork 0
Code Issues 0 Pull Requests 0 Releases 0 Wiki Activity
ROS Components for hexapod_robot
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
16 Commits
1 Branch
86KB
Branch: main
Branches Tags
${ item.name }
Create branch ${ searchTerm }
from 'main'
${ noResults }
hexapod_robot/hardware/hexapod_robot_hwi.cpp

214 lines
9.7KB
Raw Permalink Blame History 

  1. #include "hexapod_robot/hexapod_robot_hwi.hpp"

  2. 

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

  4. #include "rclcpp/rclcpp.hpp"

  5. 

  6. #include <fcntl.h>

  7. #include <unistd.h>

  8. #include <stdlib.h>

  9. #include <stdio.h>

  10. #include <fcntl.h>

  11. #include <stdint.h>

  12. 

  13. #include <sys/ioctl.h>

  14. #include <linux/i2c-dev.h>

  15. 

  16. extern "C" {

  17.     #include <i2c/smbus.h>

  18. }

  19. 

  20. namespace hexapod_robot {

  21. 

  22. 

  23. HexapodRobotHardwareInterface::~HexapodRobotHardwareInterface() {

  24.       on_deactivate(rclcpp_lifecycle::State());

  25. }

  26. 

  27. hardware_interface::CallbackReturn 

  28. HexapodRobotHardwareInterface::on_init(const hardware_interface::HardwareInfo &info) {

  29.     if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) {

  30.         return hardware_interface::CallbackReturn::ERROR;

  31.     }

  32. 

  33.     RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "on_init");

  34.     

  35.     servos_powered_ = false;

  36. 

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

  38. 

  39.         std::string i2c_address_parameter = joint.name + "_i2c_address";

  40.         std::string channel_parameter = joint.name + "_channel";

  41.         std::string counts_intersect_parameter = joint.name + "_counts_intersect";

  42.         std::string counts_slope_parameter = joint.name + "_counts_slope";

  43.         std::string counts_min_parameter = joint.name + "_counts_min";

  44.         std::string counts_max_parameter = joint.name + "_counts_max";

  45.         std::string startup_value_parameter = joint.name + "_startup_value";

  46. 

  47.         PWMOutput pwm_output;

  48.         pwm_output.i2c_address      = stoi(info_.hardware_parameters[i2c_address_parameter], nullptr, 0);

  49.         pwm_output.channel          = stoi(info_.hardware_parameters[channel_parameter], nullptr, 0);

  50.         pwm_output.counts_intersect = stod(info_.hardware_parameters[counts_intersect_parameter]);

  51.         pwm_output.counts_slope     = stod(info_.hardware_parameters[counts_slope_parameter]);

  52.         pwm_output.counts_min       = stod(info_.hardware_parameters[counts_min_parameter]);

  53.         pwm_output.counts_max       = stod(info_.hardware_parameters[counts_max_parameter]);

  54.         joint_commands_[joint.name] = stod(info_.hardware_parameters[startup_value_parameter]);

  55. 

  56.         pca_available_[pwm_output.i2c_address] = false;

  57.         joint_pwm_mapping_[joint.name] = pwm_output;

  58. 

  59.         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);

  60.     }

  61. 

  62. 

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

  64.     if(i2c_available_) pca_initialize_();

  65. 

  66.     return hardware_interface::CallbackReturn::SUCCESS;

  67. }

  68. 

  69. bool

  70. HexapodRobotHardwareInterface::i2c_open_(const std::string& device) {

  71.     i2c_fd_ = open(device.c_str(), O_RDWR);

  72.     if (i2c_fd_ < 0) {

  73.         RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "Cannot open %s, continuing in dummy mode.", info_.hardware_parameters["i2c_device"].c_str());

  74.         return false;

  75.     }

  76.     return true;

  77. }

  78. 

  79. void

  80. HexapodRobotHardwareInterface::pca_initialize_() {

  81.     for (const auto& [i2c_address, available]: pca_available_) {        

  82. 

  83.         ioctl(i2c_fd_, I2C_SLAVE, i2c_address);

  84. 

  85.         if(i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode1_, 0x00) < 0) {

  86.             RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "Could not open i2c device %02x, marking as unavailable.", i2c_address);

  87.             pca_available_[i2c_address] = false;

  88.         } else {

  89.             i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode1_,    pca_mode1_sleep_); 

  90.             i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode2_,    pca_mode2_outdrv_);

  91.             i2c_smbus_write_byte_data(i2c_fd_, pca_reg_prescale_, (uint8_t)pca_prescaler_value_);

  92.             i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode1_,    pca_mode1_restart_);

  93. 

  94.             RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "PCA9685 at i2c 0x%02x initialized.", i2c_address);

  95.             pca_available_[i2c_address] = true;

  96.         }

  97.     }

  98. }

  99. 

  100. bool

  101. HexapodRobotHardwareInterface::pca_set_pwm_(uint8_t i2c_address, uint8_t channel_number, uint16_t counts) {

  102.     // if(i2c_address == 0x40 && channel_number == 0) {

  103.     //    RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm(%02x, %02x, %04x)", i2c_address, channel_number, counts);

  104.     // }

  105.     if(channel_number>15) {

  106.         RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm: channel %02x >15", channel_number);

  107.         return false;

  108.     }

  109.     if(counts > 0x1000) {

  110.         RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm: counts %04x >15", counts);

  111.         return false;

  112.     }

  113.     if(!pca_available_[i2c_address]) {

  114.     //    RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm: i2c_address %02x unavailable.", i2c_address);

  115.         return false;

  116.     }

  117. 

  118.     if(ioctl(i2c_fd_, I2C_SLAVE, i2c_address) < 0) {

  119.         RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm: ioctl failed.");

  120.         return false;

  121.     }

  122. 

  123.     // adding channel_number * 4 to the base address for LED0 output given by the constants

  124.     // yields the addresses of all PWM channels

  125. 

  126.     if(counts == 0) {

  127.         // output always off

  128.         i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_l_  + channel_number * 4, 0x00); // ON_L

  129.         i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_h_  + channel_number * 4, 0x00); // ON_H

  130.         i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_l_ + channel_number * 4, 0x00); // OFF_L

  131.         i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_h_ + channel_number * 4, 0x10); // OFF_H == 0x10 => LED full OFF 

  132.     } else if(counts == 0x1000) {

  133.         // output always on

  134.         i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_l_  + channel_number * 4, 0x00); // ON_L

  135.         i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_h_  + channel_number * 4, 0x10); // ON_H  == 0x10 => LED full ON 

  136.         i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_l_ + channel_number * 4, 0x00); // OFF_L

  137.         i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_h_ + channel_number * 4, 0x00); // OFF_H

  138.     } else {

  139.         // duty cycle from 0 to counts

  140.         i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_h_  + channel_number * 4, 0x00); 

  141.         i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_l_  + channel_number * 4, 0x00); 

  142.         i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_l_ + channel_number * 4, counts & 0xff); // lower byte

  143.         i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_h_ + channel_number * 4, counts >>   8); // shift upper byte into lower byte

  144.     }

  145.     return true;

  146. }

  147. 

  148. hardware_interface::CallbackReturn

  149. HexapodRobotHardwareInterface::on_activate(const rclcpp_lifecycle::State &) {

  150.     RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "on_activate");

  151.     servos_powered_ = true;

  152.     return hardware_interface::CallbackReturn::SUCCESS;

  153. }

  154. 

  155. hardware_interface::CallbackReturn

  156. HexapodRobotHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &) {

  157.     RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "on_deactivate");

  158.     servos_powered_ = false;

  159.     write(rclcpp::Time(), rclcpp::Duration(std::chrono::nanoseconds(0)));

  160.     return hardware_interface::CallbackReturn::SUCCESS;

  161. }

  162. 

  163. 

  164. std::vector<hardware_interface::StateInterface>

  165. HexapodRobotHardwareInterface::export_state_interfaces() {

  166.     std::vector<hardware_interface::StateInterface> state_interfaces;

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

  168.         state_interfaces.emplace_back(hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_POSITION, &joint_commands_[joint.name]));

  169.     }

  170.     return state_interfaces;

  171. }

  172. 

  173. std::vector<hardware_interface::CommandInterface>

  174. HexapodRobotHardwareInterface::export_command_interfaces() {

  175.     std::vector<hardware_interface::CommandInterface> command_interfaces;

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

  177.         command_interfaces.emplace_back(hardware_interface::CommandInterface(joint.name, hardware_interface::HW_IF_POSITION, &joint_commands_[joint.name]));

  178.     }

  179.     return command_interfaces;

  180. }

  181. 

  182. 

  183. hardware_interface::return_type

  184. HexapodRobotHardwareInterface::read(const rclcpp::Time &, const rclcpp::Duration &) {

  185.     return hardware_interface::return_type::OK;

  186. }

  187. 

  188. hardware_interface::return_type

  189. HexapodRobotHardwareInterface::write(const rclcpp::Time &, const rclcpp::Duration &) {

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

  191.         const PWMOutput &pwm_output = joint_pwm_mapping_[joint.name];

  192.         if(servos_powered_) {

  193.             double pwm_value = pwm_output.counts_intersect + joint_commands_[joint.name] * pwm_output.counts_slope;

  194.             if(pwm_value < pwm_output.counts_min) {

  195.                 RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "write: pwm_value %.2f too low, clipped to %.2f", pwm_value, pwm_output.counts_min);

  196.                 pwm_value = pwm_output.counts_min;

  197.             }

  198.             if(pwm_value > pwm_output.counts_max) {

  199.                 RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "write: pwm_value %.2f too high, clipped to %.2f", pwm_value, pwm_output.counts_max);

  200.                 pwm_value = pwm_output.counts_max;

  201.             }

  202.             pca_set_pwm_(pwm_output.i2c_address, pwm_output.channel, (uint16_t)pwm_value);

  203.         } else {

  204.             pca_set_pwm_(pwm_output.i2c_address, pwm_output.channel, 0);

  205.         }

  206.     }    

  207.     return hardware_interface::return_type::OK;

  208. }

  209. 

  210. } // namespace

  211. 

  212. #include "pluginlib/class_list_macros.hpp"

  213. PLUGINLIB_EXPORT_CLASS(hexapod_robot::HexapodRobotHardwareInterface, hardware_interface::SystemInterface)