diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index e81e2c2f..1305524c 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -39,7 +39,7 @@ sentry_hardware: board_serial_bottom_board: "af-79cf" board_serial_gimbal_board: "af-8b8b" - pitch_motor_zero_point: 12347 + pitch_motor_zero_point: 12158 bottom_yaw_motor_zero_point: 54551 top_yaw_motor_zero_point: 33409 @@ -61,22 +61,22 @@ gimbal_controller: upper_limit: -0.65 lower_limit: 0.36 - top_yaw_angle_kp: 45.0 - top_yaw_angle_ki: 0.0 - top_yaw_angle_kd: 0.0 + top_yaw_angle_kp: 35.0 + top_yaw_angle_ki: 0.008 + top_yaw_angle_kd: 0.005 top_yaw_velocity_kp: 2.160 - top_yaw_velocity_ki: 0.0 + top_yaw_velocity_ki: 0.00 top_yaw_velocity_kd: 0.0 - bottom_yaw_angle_kp: 12.0 - bottom_yaw_angle_ki: 0.0 - bottom_yaw_angle_kd: 0.0 - bottom_yaw_velocity_kp: 22.5 - bottom_yaw_velocity_ki: 0.0 - bottom_yaw_velocity_kd: 0.0 + bottom_yaw_angle_kp: 9.5 + bottom_yaw_angle_ki: 0.001 + bottom_yaw_angle_kd: 0.0003 + bottom_yaw_velocity_kp: 18.0 + bottom_yaw_velocity_ki: 0.00 + bottom_yaw_velocity_kd: 0.00 pitch_angle_kp: 32.0 - pitch_angle_ki: 0.0 + pitch_angle_ki: 0.01 pitch_angle_kd: 0.0 pitch_velocity_kp: 2.5 pitch_velocity_ki: 0.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp index 5dfff94e..d056c389 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp @@ -54,11 +54,27 @@ class EccentricDualYaw const auto xy_norm = std::hypot(dir.x(), dir.y()); const auto pitch = std::clamp(std::atan2(-dir.z(), xy_norm), upper_limit_, lower_limit_); - const auto yaw = std::atan2(dir.y(), dir.x()); + const auto control_yaw = std::atan2(dir.y(), dir.x()); + + const auto camera_pose = fast_tf::lookup_transform(*input_.tf); + const auto camera_pos = camera_pose.translation(); + const auto robot_center = *input_.auto_aim_robot_center; + Eigen::Vector3d diff{ + robot_center.x() - camera_pos.x(), + robot_center.y() - camera_pos.y(), + robot_center.z() - camera_pos.z(), + }; + double center_yaw; + if (diff.norm() > 1e-9) { + diff.normalize(); + center_yaw = std::atan2(diff.y(), diff.x()); + } else { + center_yaw = 0.0; + } const auto command = ControlTarget{ - .bottom_yaw = {.target = yaw}, - .top_yaw = {.target = 0.0}, + .bottom_yaw = {.target = center_yaw}, + .top_yaw = {.target = limit_rad(control_yaw - center_yaw)}, .pitch = {.target = pitch}, }; apply_control(command); @@ -170,6 +186,7 @@ class EccentricDualYaw component.register_input("/auto_aim/pitch_rate", auto_aim_pitch_rate, false); component.register_input("/auto_aim/yaw_acc", auto_aim_yaw_acc, false); component.register_input("/auto_aim/pitch_acc", auto_aim_pitch_acc, false); + component.register_input("/auto_aim/robot_center", auto_aim_robot_center, false); component.register_input( "/rmcs_navigation/enable_control", navigation_enable_control, false); component.register_input("/rmcs_navigation/gimbal_toward", navigation_toward, false); @@ -191,8 +208,12 @@ class EccentricDualYaw if (!auto_aim_control_direction.ready()) return false; const auto& dir = *auto_aim_control_direction; + if (!auto_aim_robot_center.ready()) + return false; + const auto& center = *auto_aim_robot_center; return !dir.isZero() && std::isfinite(dir.x()) && std::isfinite(dir.y()) - && std::isfinite(dir.z()); + && std::isfinite(dir.z()) && !center.isZero() && std::isfinite(center.x()) + && std::isfinite(center.y()) && std::isfinite(center.z()); } auto enable_navigation() const noexcept -> bool { @@ -220,6 +241,7 @@ class EccentricDualYaw InputInterface chassis_yaw_velocity_imu; InputInterface auto_aim_control_direction; + InputInterface auto_aim_robot_center; InputInterface auto_aim_yaw_rate; InputInterface auto_aim_pitch_rate; InputInterface auto_aim_yaw_acc; @@ -379,12 +401,12 @@ class EccentricDualYaw pitch_velocity_ref) + pitch_gravity_ff_gain_ * std::sin(current_pitch_angle - pitch_gravity_ff_phase_); - *output_.bottom_yaw_control_torque = - bottom_yaw_velocity_pid_.update(bottom_velocity_ref - current_bottom_velocity) - + bottom_yaw_torque_ff; *output_.top_yaw_control_torque = top_yaw_velocity_pid_.update(top_velocity_ref - *input_.top_yaw_velocity) + top_yaw_torque_ff; + *output_.bottom_yaw_control_torque = + bottom_yaw_velocity_pid_.update(bottom_velocity_ref - current_bottom_velocity) + + bottom_yaw_torque_ff; *output_.pitch_control_torque = pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity) + pitch_torque_ff;