From bdc18f684a68ada37d8b850f63806cc235e31f3e Mon Sep 17 00:00:00 2001 From: Rayy0527 <1668834312@qq.com> Date: Fri, 6 Dec 2024 18:52:13 +0800 Subject: [PATCH] Standardlize some code. --- .../rm_manual/chassis_gimbal_shooter_cover_manual.h | 5 +---- src/chassis_gimbal_shooter_cover_manual.cpp | 10 ++++------ 2 files changed, 5 insertions(+), 10 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h index 2e2ff694..f60e54d8 100644 --- a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h @@ -53,10 +53,7 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual double low_speed_scale_{}, normal_speed_scale_{}; double exit_buff_mode_duration_{}; double gyro_speed_limit_{}; - double sin_gyro_base_scale_{ 1. }; - double sin_gyro_amplitude_{ 0. }; - double sin_gyro_period_{ 1. }; - double sin_gyro_phase_{ 0. }; + double sin_gyro_base_scale_{ 1. }, sin_gyro_amplitude_{ 0. }, sin_gyro_period_{ 1. }, sin_gyro_phase_{ 0. }; rm_common::SwitchDetectionCaller* switch_buff_srv_{}; rm_common::SwitchDetectionCaller* switch_buff_type_srv_{}; rm_common::SwitchDetectionCaller* switch_exposure_srv_{}; diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index d7f8b0d7..fa749fa3 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -47,7 +47,8 @@ void ChassisGimbalShooterCoverManual::changeSpeedMode(SpeedMode speed_mode) speed_change_scale_ = normal_speed_scale_; } } -double ChassisGimbalShooterCoverManual::getDynamicScale(double base_scale, double amplitude, double period, double phase) +double ChassisGimbalShooterCoverManual::getDynamicScale(const double base_scale, const double amplitude, + const double period, const double phase) { ros::Time current_time = ros::Time::now(); double t = current_time.toSec(); @@ -75,12 +76,9 @@ void ChassisGimbalShooterCoverManual::changeGyroSpeedMode(SpeedMode speed_mode) else if (speed_mode == NORMAL) { if (x_scale_ != 0.0 || y_scale_ != 0.0) - vel_cmd_sender_->setAngularZVel( - getDynamicScale(sin_gyro_base_scale_, sin_gyro_amplitude_, sin_gyro_period_, sin_gyro_phase_) * - gyro_rotate_reduction_); + vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_); else - vel_cmd_sender_->setAngularZVel( - getDynamicScale(sin_gyro_base_scale_, sin_gyro_amplitude_, sin_gyro_period_, sin_gyro_phase_)); + vel_cmd_sender_->setAngularZVel(1.0); } }