Fixes and initial calibration

description/urdf/hexapod_robot_macros.xacro

@@ -90,7 +90,7 @@
             <child  link="${name}_link"/>
             <xacro:if value="${side == 'l'}"><origin xyz="-0.020 0.03 -0.0075"/></xacro:if>
             <xacro:if value="${side == 'r'}"><origin xyz=" 0.020 0.03 -0.0075"/></xacro:if>
-            <axis xyz="0 1 0"/>
+            <axis xyz="0 -1 0"/>
             <limit effort="30" velocity="100" lower="-1" upper="1"/>
         </joint>
     </xacro:macro>

description/urdf/hexapod_robot_ros2_control.xacro

@@ -27,11 +27,13 @@
         </joint>
     </xacro:macro>
 
-    <xacro:macro name="map_joint_name_to_hardware" params="name i2c_address channel counts_intersect counts_slope">
+    <xacro:macro name="map_joint_name_to_hardware" params="name i2c_address channel counts_intersect counts_slope counts_min counts_max">
         <param name="${name}_joint_i2c_address">${i2c_address}</param>
         <param name="${name}_joint_channel">${channel}</param>
         <param name="${name}_joint_counts_intersect">${counts_intersect}</param>
         <param name="${name}_joint_counts_slope">${counts_slope}</param>
+        <param name="${name}_joint_counts_min">${counts_min}</param>
+        <param name="${name}_joint_counts_max">${counts_max}</param>
     </xacro:macro>
 
     <ros2_control name="Hexapod" type="system">
@@ -39,29 +41,29 @@
         <hardware>
             <plugin>hexapod_robot/HexapodRobotHardwareInterface</plugin>
             <param name="i2c_device">/dev/i2c-1</param>
-            <xacro:map_joint_name_to_hardware name="c1" i2c_address="0x40" channel="13" counts_intersect="307" counts_slope="129"/>
-            <xacro:map_joint_name_to_hardware name="f1" i2c_address="0x40" channel="14" counts_intersect="307" counts_slope="129"/>
-            <xacro:map_joint_name_to_hardware name="t1" i2c_address="0x40" channel="15" counts_intersect="307" counts_slope="129"/>
+            <xacro:map_joint_name_to_hardware name="c1" i2c_address="0x40" channel="13" counts_intersect="386" counts_slope="129" counts_min="125" counts_max="550"/>
+            <xacro:map_joint_name_to_hardware name="f1" i2c_address="0x40" channel="14" counts_intersect="307" counts_slope="129" counts_min="125" counts_max="550"/>
+            <xacro:map_joint_name_to_hardware name="t1" i2c_address="0x40" channel="15" counts_intersect="189" counts_slope="129" counts_min="125" counts_max="550"/>
 
-            <xacro:map_joint_name_to_hardware name="c2" i2c_address="0x40" channel="9"  counts_intersect="307" counts_slope="129"/>
-            <xacro:map_joint_name_to_hardware name="f2" i2c_address="0x40" channel="10" counts_intersect="307" counts_slope="129"/>
-            <xacro:map_joint_name_to_hardware name="t2" i2c_address="0x40" channel="11" counts_intersect="307" counts_slope="129"/>
-
-            <xacro:map_joint_name_to_hardware name="c3" i2c_address="0x40" channel="2"  counts_intersect="307" counts_slope="129"/>
-            <xacro:map_joint_name_to_hardware name="f3" i2c_address="0x40" channel="1"  counts_intersect="307" counts_slope="129"/>
-            <xacro:map_joint_name_to_hardware name="t3" i2c_address="0x40" channel="0"  counts_intersect="307" counts_slope="129"/>
-
-            <xacro:map_joint_name_to_hardware name="c4" i2c_address="0x41" channel="13" counts_intersect="307" counts_slope="129"/>
-            <xacro:map_joint_name_to_hardware name="f4" i2c_address="0x41" channel="14" counts_intersect="307" counts_slope="129"/>
-            <xacro:map_joint_name_to_hardware name="t4" i2c_address="0x41" channel="15" counts_intersect="307" counts_slope="129"/>
-
-            <xacro:map_joint_name_to_hardware name="c5" i2c_address="0x41" channel="6"  counts_intersect="307" counts_slope="129"/>
-            <xacro:map_joint_name_to_hardware name="f5" i2c_address="0x41" channel="5"  counts_intersect="307" counts_slope="129"/>
-            <xacro:map_joint_name_to_hardware name="t5" i2c_address="0x41" channel="4"  counts_intersect="307" counts_slope="129"/>
-
-            <xacro:map_joint_name_to_hardware name="c6" i2c_address="0x41" channel="2"  counts_intersect="307" counts_slope="129"/>
-            <xacro:map_joint_name_to_hardware name="f6" i2c_address="0x41" channel="1"  counts_intersect="307" counts_slope="129"/>
-            <xacro:map_joint_name_to_hardware name="t6" i2c_address="0x41" channel="0"  counts_intersect="307" counts_slope="129"/>
+            <xacro:map_joint_name_to_hardware name="c2" i2c_address="0x40" channel="9"  counts_intersect="327" counts_slope="129" counts_min="125" counts_max="550"/>
+            <xacro:map_joint_name_to_hardware name="f2" i2c_address="0x40" channel="10" counts_intersect="317" counts_slope="129" counts_min="125" counts_max="550"/>
+            <xacro:map_joint_name_to_hardware name="t2" i2c_address="0x40" channel="11" counts_intersect="227" counts_slope="129" counts_min="125" counts_max="550"/>
+            
+            <xacro:map_joint_name_to_hardware name="c3" i2c_address="0x40" channel="2"  counts_intersect="237" counts_slope="129" counts_min="125" counts_max="550"/>
+            <xacro:map_joint_name_to_hardware name="f3" i2c_address="0x40" channel="1"  counts_intersect="307" counts_slope="129" counts_min="125" counts_max="550"/>
+            <xacro:map_joint_name_to_hardware name="t3" i2c_address="0x40" channel="0"  counts_intersect="267" counts_slope="129" counts_min="125" counts_max="550"/>
+            
+            <xacro:map_joint_name_to_hardware name="c4" i2c_address="0x41" channel="13" counts_intersect="451" counts_slope="129" counts_min="125" counts_max="550"/>
+            <xacro:map_joint_name_to_hardware name="f4" i2c_address="0x41" channel="14" counts_intersect="367" counts_slope="129" counts_min="125" counts_max="550"/>
+            <xacro:map_joint_name_to_hardware name="t4" i2c_address="0x41" channel="15" counts_intersect="487" counts_slope="129" counts_min="125" counts_max="550"/>
+            
+            <xacro:map_joint_name_to_hardware name="c5" i2c_address="0x41" channel="6"  counts_intersect="357" counts_slope="129" counts_min="125" counts_max="550"/>
+            <xacro:map_joint_name_to_hardware name="f5" i2c_address="0x41" channel="5"  counts_intersect="367" counts_slope="129" counts_min="125" counts_max="550"/>
+            <xacro:map_joint_name_to_hardware name="t5" i2c_address="0x41" channel="4"  counts_intersect="437" counts_slope="129" counts_min="125" counts_max="550"/>
+            
+            <xacro:map_joint_name_to_hardware name="c6" i2c_address="0x41" channel="2"  counts_intersect="255" counts_slope="129" counts_min="125" counts_max="550"/>
+            <xacro:map_joint_name_to_hardware name="f6" i2c_address="0x41" channel="1"  counts_intersect="335" counts_slope="129" counts_min="125" counts_max="550"/>
+            <xacro:map_joint_name_to_hardware name="t6" i2c_address="0x41" channel="0"  counts_intersect="444" counts_slope="129" counts_min="125" counts_max="550"/>
         </hardware>
 
         <xacro:create_leg_joints number="1"/>    

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;
 }

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<std::string, double> joint_commands_;
+    bool servos_powered_;
 
+    std::map<std::string, double> joint_commands_;
     std::map<std::string, PWMOutput> joint_pwm_mapping_;
 
     bool i2c_available_;