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

213 lines
9.5KB
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. 

  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.         joint_commands_[joint.name] = 0.0;

  39. 

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

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

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

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

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

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

  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. 

  55.         pca_available_[pwm_output.i2c_address] = false;

  56.         joint_pwm_mapping_[joint.name] = pwm_output;

  57. 

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

  59.     }

  60. 

  61. 

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

  63.     pca_initialize_();

  64. 

  65.     return hardware_interface::CallbackReturn::SUCCESS;

  66. }

  67. 

  68. bool

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

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

  71.     if (i2c_fd_ < 0) {

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

  73.         return false;

  74.     }

  75.     return true;

  76. }

  77. 

  78. void

  79. HexapodRobotHardwareInterface::pca_initialize_() {

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

  81. 

  82.         ioctl(i2c_fd_, I2C_SLAVE, i2c_address);

  83. 

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

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

  86.             pca_available_[i2c_address] = false;

  87.         } else {

  88.             i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode1_,    pca_mode1_sleep_); 

  89.             i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode2_,    pca_mode2_outdrv_);

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

  91.             i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode1_,    pca_mode1_restart_);

  92. 

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

  94.             pca_available_[i2c_address] = true;

  95.         }

  96.     }

  97. }

  98. 

  99. bool

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

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

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

  103.     // }

  104.     if(channel_number>15) {

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

  106.         return false;

  107.     }

  108.     if(counts > 0x1000) {

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

  110.         return false;

  111.     }

  112.     if(!pca_available_[i2c_address]) {

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

  114.         return false;

  115.     }

  116. 

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

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

  119.         return false;

  120.     }

  121. 

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

  123.     // yields the addresses of all PWM channels

  124. 

  125.     if(counts == 0) {

  126.         // output always off

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

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

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

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

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

  132.         // output always on

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

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

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

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

  137.     } else {

  138.         // duty cycle from 0 to counts

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

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

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

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

  143.     }

  144.     return true;

  145. }

  146. 

  147. hardware_interface::CallbackReturn

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

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

  150.     servos_powered_ = true;

  151.     return hardware_interface::CallbackReturn::SUCCESS;

  152. }

  153. 

  154. hardware_interface::CallbackReturn

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

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

  157.     servos_powered_ = false;

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

  159.     return hardware_interface::CallbackReturn::SUCCESS;

  160. }

  161. 

  162. 

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

  164. HexapodRobotHardwareInterface::export_state_interfaces() {

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

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

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

  168.     }

  169.     return state_interfaces;

  170. }

  171. 

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

  173. HexapodRobotHardwareInterface::export_command_interfaces() {

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

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

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

  177.     }

  178.     return command_interfaces;

  179. }

  180. 

  181. 

  182. hardware_interface::return_type

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

  184.     return hardware_interface::return_type::OK;

  185. }

  186. 

  187. hardware_interface::return_type

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

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

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

  191.         if(servos_powered_) {

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

  193.             if(pwm_value < pwm_output.counts_min) {

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

  195.                 pwm_value = pwm_output.counts_min;

  196.             }

  197.             if(pwm_value > pwm_output.counts_max) {

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

  199.                 pwm_value = pwm_output.counts_max;

  200.             }

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

  202.         } else {

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

  204.         }

  205.     }    

  206.     return hardware_interface::return_type::OK;

  207. }

  208. 

  209. } // namespace

  210. 

  211. #include "pluginlib/class_list_macros.hpp"

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