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.
3 Commits
1 Branch
86KB
Tree: 7e012eb577
Branches Tags
${ item.name }
Create branch ${ searchTerm }
from '7e012eb577'
${ noResults }
hexapod_robot/hardware/hexapod_robot_hwi.cpp

189 lines
8.3KB
Raw 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. hardware_interface::CallbackReturn 

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

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

  25.         return hardware_interface::CallbackReturn::ERROR;

  26.     }

  27. 

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

  29.     

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

  31.         joint_states_[joint.name]= 0.0;

  32.         joint_commands_[joint.name] = 0.0;

  33. 

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

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

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

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

  38. 

  39. 

  40.         PWMOutput pwm_output;

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

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

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

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

  45. 

  46.         pca_available_[pwm_output.i2c_address] = false;

  47.         joint_pwm_mapping_[joint.name] = pwm_output;

  48. 

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

  50.     }

  51. 

  52. 

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

  54.     pca_initialize_();

  55. 

  56.     return hardware_interface::CallbackReturn::SUCCESS;

  57. }

  58. 

  59. bool

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

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

  62.     if (i2c_fd_ < 0) {

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

  64.         return false;

  65.     }

  66.     return true;

  67. }

  68. 

  69. void

  70. HexapodRobotHardwareInterface::pca_initialize_() {

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

  72. 

  73.         ioctl(i2c_fd_, I2C_SLAVE, i2c_address);

  74. 

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

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

  77.             pca_available_[i2c_address] = false;

  78.         } else {

  79.             i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode1_,    pca_mode1_sleep_); 

  80.             i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode2_,    pca_mode2_outdrv_);

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

  82.             i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode1_,    pca_mode1_restart_);

  83. 

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

  85.             pca_available_[i2c_address] = true;

  86.         }

  87.     }

  88. }

  89. 

  90. bool

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

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

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

  94.     // }

  95.     if(channel_number>15) {

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

  97.         return false;

  98.     }

  99.     if(counts > 0x1000) {

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

  101.         return false;

  102.     }

  103.     if(!pca_available_[i2c_address]) {

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

  105.         return false;

  106.     }

  107. 

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

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

  110.         return false;

  111.     }

  112. 

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

  114.     // yields the addresses of all PWM channels

  115. 

  116.     if(counts == 0) {

  117.         // output always off

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

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

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

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

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

  123.         // output always on

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

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

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

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

  128.     } else {

  129.         // duty cycle from 0 to counts

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

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

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

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

  134.     }

  135.     return true;

  136. }

  137. 

  138. hardware_interface::CallbackReturn

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

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

  141.     return hardware_interface::CallbackReturn::SUCCESS;

  142. }

  143. 

  144. hardware_interface::CallbackReturn

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

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

  147.     return hardware_interface::CallbackReturn::SUCCESS;

  148. }

  149. 

  150. 

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

  152. HexapodRobotHardwareInterface::export_state_interfaces() {

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

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

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

  156.     }

  157.     return state_interfaces;

  158. }

  159. 

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

  161. HexapodRobotHardwareInterface::export_command_interfaces() {

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

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

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

  165.     }

  166.     return command_interfaces;

  167. }

  168. 

  169. 

  170. hardware_interface::return_type

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

  172.     return hardware_interface::return_type::OK;

  173. }

  174. 

  175. hardware_interface::return_type

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

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

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

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

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

  181.     }    

  182.     return hardware_interface::return_type::OK;

  183. }

  184. 

  185. } // namespace

  186. 

  187. #include "pluginlib/class_list_macros.hpp"

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