update gimbal_position_controller to mit velocity mode

Author: tz61

decomposition/meta_gimbal_controller/include/gimbal_controller/gimbal_position_controller.hpp

@@ -74,9 +74,6 @@ class GimbalPositionController : public controller_interface::ChainableControlle
         input_feedback_;
 
     std::shared_ptr<control_toolbox::PidROS> yaw_pos2vel_pid_;
-    std::shared_ptr<control_toolbox::PidROS> pitch_pos2vel_pid_;
-    std::shared_ptr<control_toolbox::PidROS> yaw_vel2eff_pid_;
-    std::shared_ptr<control_toolbox::PidROS> pitch_vel2eff_pid_;
 
     using ControllerStatePublisher =
         realtime_tools::RealtimePublisher<ControllerStateMsg>;

decomposition/meta_gimbal_controller/src/gimbal_position_controller.cpp

@@ -258,6 +258,7 @@ GimbalPositionController::update_and_write_commands(const rclcpp::Time &time,
             // for cybergear negative up, for imu negative up, for reference it should be negative up too
             pitch_pos_ref = -reference_interfaces_[1];
             pitch_pos_err = angles::shortest_angular_distance(pitch_pos_fb, pitch_pos_ref);
+            command_interfaces_[3].set_value(pitch_enc_pos + pitch_pos_err);
         }
     }
     // Publish state

decomposition/metav_description/urdf/hero/hero.xacro

@@ -54,7 +54,12 @@
                 <param name="master_id">48</param> <!-- 0x30-->
             </hardware>
             <joint name="yaw_gimbal_joint">
-                <command_interface name="position" />
+                <command_interface name="position"/>
+                <command_interface name="velocity">
+                    <param name="min">-0.1</param>
+                    <param name="max">0.1</param>
+                </command_interface>
+                <command_interface name="effort"/>
                 <state_interface name="position" />
                 <state_interface name="velocity" />
                 <state_interface name="effort" />
@@ -66,7 +71,7 @@
                 <param name="max_effort">12.0</param>
                 <param name="offset">0.0</param>
                 <param name="control_mode">mit</param>
-                <param name="Kp">5.0</param>
+                <param name="Kp">0.0</param>
                 <param name="Kd">0.5</param>
             </joint>
         </ros2_control>

meta_bringup/config/hero.yaml

@@ -83,6 +83,9 @@ gimbal_position_controller:
       name: pitch_gimbal_joint
 
     imu_topic: /imu
+    gains:
+      yaw_gimbal_joint_pos2vel:
+        { p: 7.0, i: 0.0, d: 0.1, i_clamp_max: 1.0, i_clamp_min: -1.0, antiwindup: true }
 
 
 shoot_controller: