From 9b081f586e0829e630fbc37405e36455082fa086 Mon Sep 17 00:00:00 2001 From: Rayy0527 <1668834312@qq.com> Date: Sun, 24 Nov 2024 15:51:53 +0800 Subject: [PATCH 1/5] Add sin speed gyro. --- .../chassis_gimbal_shooter_cover_manual.h | 5 ++++ src/chassis_gimbal_shooter_cover_manual.cpp | 29 +++++++++++++++++-- 2 files changed, 31 insertions(+), 3 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h index cb4203e8..2e2ff694 100644 --- a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h @@ -20,6 +20,7 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual protected: void changeSpeedMode(SpeedMode speed_mode); + double getDynamicScale(double base_scale, double amplitude, double period, double phase); void changeGyroSpeedMode(SpeedMode speed_mode); void updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) override; void checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) override; @@ -52,6 +53,10 @@ 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. }; 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 73a92802..0e5c1c74 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -23,6 +23,11 @@ ChassisGimbalShooterCoverManual::ChassisGimbalShooterCoverManual(ros::NodeHandle low_speed_scale_ = chassis_nh.param("low_speed_scale", 0.30); nh.param("exit_buff_mode_duration", exit_buff_mode_duration_, 0.5); nh.param("gyro_speed_limit", gyro_speed_limit_, 6.0); + ros::NodeHandle vel_nh(nh, "vel"); + sin_gyro_base_scale_ = vel_nh.param("sin_gyro_base_scale", 1.0); + sin_gyro_amplitude_ = vel_nh.param("sin_gyro_amplitude", 0.0); + sin_gyro_period_ = vel_nh.param("sin_gyro_period", 1.0); + sin_gyro_phase_ = vel_nh.param("sin_gyro_phase", 0.0); ctrl_z_event_.setEdge(boost::bind(&ChassisGimbalShooterCoverManual::ctrlZPress, this), boost::bind(&ChassisGimbalShooterCoverManual::ctrlZRelease, this)); @@ -42,7 +47,22 @@ void ChassisGimbalShooterCoverManual::changeSpeedMode(SpeedMode speed_mode) speed_change_scale_ = normal_speed_scale_; } } - +double ChassisGimbalShooterCoverManual::getDynamicScale(double base_scale, double amplitude, double period, double phase) +{ + ros::Time current_time = ros::Time::now(); + double t = current_time.toSec(); + double f = 2 * M_PI / period; + double dynamic_scale = base_scale + amplitude * sin(f * t + phase); + if (dynamic_scale < 0.0) + { + dynamic_scale = 0.0; + } + else if (dynamic_scale > 1.0) + { + dynamic_scale = 1.0; + } + return dynamic_scale; +} void ChassisGimbalShooterCoverManual::changeGyroSpeedMode(SpeedMode speed_mode) { if (speed_mode == LOW) @@ -55,9 +75,12 @@ void ChassisGimbalShooterCoverManual::changeGyroSpeedMode(SpeedMode speed_mode) else if (speed_mode == NORMAL) { if (x_scale_ != 0.0 || y_scale_ != 0.0) - vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_); + vel_cmd_sender_->setAngularZVel( + getDynamicScale(sin_gyro_base_scale_, sin_gyro_amplitude_, sin_gyro_period_, sin_gyro_phase_) * + gyro_rotate_reduction_); else - vel_cmd_sender_->setAngularZVel(1.0); + vel_cmd_sender_->setAngularZVel( + getDynamicScale(sin_gyro_base_scale_, sin_gyro_amplitude_, sin_gyro_period_, sin_gyro_phase_)); } } From ea5e1cc0aea726d93ffd038b2c9cd18d5254220b Mon Sep 17 00:00:00 2001 From: Rayy0527 <1668834312@qq.com> Date: Sun, 1 Dec 2024 21:44:17 +0800 Subject: [PATCH 2/5] Maintain Rc wheel function. --- src/chassis_gimbal_shooter_cover_manual.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index 0e5c1c74..d7f8b0d7 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -155,6 +155,22 @@ void ChassisGimbalShooterCoverManual::sendCommand(const ros::Time& time) } ChassisGimbalShooterManual::sendCommand(time); cover_command_sender_->sendCommand(time); + if (state_ == PC && is_gyro_) + { + if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR) + if (x_scale_ != 0.0 || y_scale_ != 0.0) + vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_, gyro_speed_limit_); + else + vel_cmd_sender_->setAngularZVel(1.0, gyro_speed_limit_); + else 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_); + else + vel_cmd_sender_->setAngularZVel( + getDynamicScale(sin_gyro_base_scale_, sin_gyro_amplitude_, sin_gyro_period_, sin_gyro_phase_)); + vel_cmd_sender_->sendCommand(time); + } } void ChassisGimbalShooterCoverManual::rightSwitchDownRise() 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 3/5] 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); } } From c4208e055bb9a951bc0adc217f4dc3cf8a974129 Mon Sep 17 00:00:00 2001 From: Rayy0527 <1668834312@qq.com> Date: Sat, 7 Dec 2024 10:21:05 +0800 Subject: [PATCH 4/5] Optimize some code. --- .../chassis_gimbal_shooter_cover_manual.h | 2 +- src/chassis_gimbal_shooter_cover_manual.cpp | 31 +++++++++---------- 2 files changed, 16 insertions(+), 17 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h index f60e54d8..7ff0e705 100644 --- a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h @@ -20,7 +20,7 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual protected: void changeSpeedMode(SpeedMode speed_mode); - double getDynamicScale(double base_scale, double amplitude, double period, double phase); + double getDynamicScale(const double base_scale, const double amplitude, const double period, const double phase); void changeGyroSpeedMode(SpeedMode speed_mode); void updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) override; void checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) override; diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index fa749fa3..1e4454c1 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -87,6 +87,21 @@ void ChassisGimbalShooterCoverManual::updatePc(const rm_msgs::DbusData::ConstPtr ChassisGimbalShooterManual::updatePc(dbus_data); gimbal_cmd_sender_->setRate(-dbus_data->m_x * gimbal_scale_, cover_command_sender_->getState() ? 0.0 : dbus_data->m_y * gimbal_scale_); + if (is_gyro_) + { + if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR) + if (x_scale_ != 0.0 || y_scale_ != 0.0) + vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_, gyro_speed_limit_); + else + vel_cmd_sender_->setAngularZVel(1.0, gyro_speed_limit_); + else 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_); + else + vel_cmd_sender_->setAngularZVel( + getDynamicScale(sin_gyro_base_scale_, sin_gyro_amplitude_, sin_gyro_period_, sin_gyro_phase_)); + } } void ChassisGimbalShooterCoverManual::checkReferee() @@ -153,22 +168,6 @@ void ChassisGimbalShooterCoverManual::sendCommand(const ros::Time& time) } ChassisGimbalShooterManual::sendCommand(time); cover_command_sender_->sendCommand(time); - if (state_ == PC && is_gyro_) - { - if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR) - if (x_scale_ != 0.0 || y_scale_ != 0.0) - vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_, gyro_speed_limit_); - else - vel_cmd_sender_->setAngularZVel(1.0, gyro_speed_limit_); - else 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_); - else - vel_cmd_sender_->setAngularZVel( - getDynamicScale(sin_gyro_base_scale_, sin_gyro_amplitude_, sin_gyro_period_, sin_gyro_phase_)); - vel_cmd_sender_->sendCommand(time); - } } void ChassisGimbalShooterCoverManual::rightSwitchDownRise() From a189adb9920e105fd9910b5e8f8129bd82b1ac31 Mon Sep 17 00:00:00 2001 From: Rayy0527 <1668834312@qq.com> Date: Sat, 7 Dec 2024 10:53:58 +0800 Subject: [PATCH 5/5] Standardlize some code. --- src/chassis_gimbal_shooter_cover_manual.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index 1e4454c1..5a15921b 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -47,6 +47,7 @@ void ChassisGimbalShooterCoverManual::changeSpeedMode(SpeedMode speed_mode) speed_change_scale_ = normal_speed_scale_; } } + double ChassisGimbalShooterCoverManual::getDynamicScale(const double base_scale, const double amplitude, const double period, const double phase) { @@ -64,6 +65,7 @@ double ChassisGimbalShooterCoverManual::getDynamicScale(const double base_scale, } return dynamic_scale; } + void ChassisGimbalShooterCoverManual::changeGyroSpeedMode(SpeedMode speed_mode) { if (speed_mode == LOW)