diff --git a/description/urdf/hexapod_robot_macros.xacro b/description/urdf/hexapod_robot_macros.xacro
index 6b1991e..59b15a9 100644
--- a/description/urdf/hexapod_robot_macros.xacro
+++ b/description/urdf/hexapod_robot_macros.xacro
@@ -90,7 +90,7 @@
-
+
diff --git a/description/urdf/hexapod_robot_ros2_control.xacro b/description/urdf/hexapod_robot_ros2_control.xacro
index bef9ca6..3cd608b 100644
--- a/description/urdf/hexapod_robot_ros2_control.xacro
+++ b/description/urdf/hexapod_robot_ros2_control.xacro
@@ -27,11 +27,13 @@
-
+
${i2c_address}
${channel}
${counts_intersect}
${counts_slope}
+ ${counts_min}
+ ${counts_max}
@@ -39,29 +41,29 @@
hexapod_robot/HexapodRobotHardwareInterface
/dev/i2c-1
-
-
-
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/hardware/hexapod_robot_hwi.cpp b/hardware/hexapod_robot_hwi.cpp
index b95d0e5..ea1395f 100644
--- a/hardware/hexapod_robot_hwi.cpp
+++ b/hardware/hexapod_robot_hwi.cpp
@@ -19,6 +19,11 @@ extern "C" {
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) {
@@ -27,21 +32,25 @@ HexapodRobotHardwareInterface::on_init(const hardware_interface::HardwareInfo &i
RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "on_init");
+ servos_powered_ = false;
+
for (const hardware_interface::ComponentInfo &joint: info_.joints) {
- joint_states_[joint.name]= 0.0;
joint_commands_[joint.name] = 0.0;
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";
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]);
pca_available_[pwm_output.i2c_address] = false;
joint_pwm_mapping_[joint.name] = pwm_output;
@@ -138,12 +147,15 @@ HexapodRobotHardwareInterface::pca_set_pwm_(uint8_t i2c_address, uint8_t channel
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;
}
@@ -175,9 +187,21 @@ HexapodRobotHardwareInterface::read(const rclcpp::Time &, const rclcpp::Duration
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];
- const double pwm_value = pwm_output.counts_intersect + joint_commands_[joint.name] * pwm_output.counts_slope;
- pca_set_pwm_(pwm_output.i2c_address, pwm_output.channel, (uint16_t)pwm_value);
+ 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;
}
diff --git a/hardware/include/hexapod_robot/hexapod_robot_hwi.hpp b/hardware/include/hexapod_robot/hexapod_robot_hwi.hpp
index 8435370..df44042 100644
--- a/hardware/include/hexapod_robot/hexapod_robot_hwi.hpp
+++ b/hardware/include/hexapod_robot/hexapod_robot_hwi.hpp
@@ -22,12 +22,15 @@ struct PWMOutput {
uint8_t channel;
double counts_intersect;
double counts_slope;
+ double counts_min;
+ double counts_max;
};
class HexapodRobotHardwareInterface : public hardware_interface::SystemInterface {
public:
RCLCPP_SHARED_PTR_DEFINITIONS(HexapodRobotHardwareInterface)
+ virtual ~HexapodRobotHardwareInterface();
hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override;
@@ -41,8 +44,9 @@ public:
hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
private:
- std::map joint_commands_;
+ bool servos_powered_;
+ std::map joint_commands_;
std::map joint_pwm_mapping_;
bool i2c_available_;