From 3f6e53e3174d7dd885ab6bbb7bb4ed423e349deb Mon Sep 17 00:00:00 2001 From: Rayy0527 <1668834312@qq.com> Date: Mon, 7 Apr 2025 05:10:17 +0800 Subject: [PATCH 1/2] Fix normalize function. --- .../include/rm_shooter_controllers/standard.h | 1 + rm_shooter_controllers/src/standard.cpp | 13 +++++++++++-- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index f2c34daa..1bad7d8e 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -94,6 +94,7 @@ class Controller : public controller_interface::MultiInterfaceController= freq_threshold_))) { + if (state_ == STOP && cmd_.mode == READY) + enter_ready_ = true; state_ = cmd_.mode; state_changed_ = true; } @@ -306,8 +308,15 @@ void Controller::setSpeed(const rm_msgs::ShootCmd& cmd) void Controller::normalize() { double push_angle = 2. * M_PI / static_cast(push_per_rotation_); - ctrl_trigger_.setCommand( - push_angle * std::floor((ctrl_trigger_.joint_.getPosition() + 0.01 + config_.exit_push_threshold) / push_angle)); + if (cmd_.hz <= freq_threshold_) + ctrl_trigger_.setCommand(push_angle * std::floor(ctrl_trigger_.joint_.getPosition() / push_angle)); + else if (enter_ready_) + { + ctrl_trigger_.setCommand(push_angle * std::floor((ctrl_trigger_.joint_.getPosition() + 0.01) / push_angle)); + enter_ready_ = false; + } + else + ctrl_trigger_.setCommand(push_angle * std::floor((ctrl_trigger_.joint_.getPosition() - 0.01) / push_angle)); } void Controller::judgeBulletShoot(const ros::Time& time, const ros::Duration& period) From 18d1dcf1faa3bd1fb1a96c7a17f847f6f2b9f567 Mon Sep 17 00:00:00 2001 From: Rayy0527 <1668834312@qq.com> Date: Mon, 7 Apr 2025 21:03:43 +0800 Subject: [PATCH 2/2] Fix normalize. --- rm_shooter_controllers/src/standard.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 1519014e..9f6823fd 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -309,7 +309,8 @@ void Controller::normalize() { double push_angle = 2. * M_PI / static_cast(push_per_rotation_); if (cmd_.hz <= freq_threshold_) - ctrl_trigger_.setCommand(push_angle * std::floor(ctrl_trigger_.joint_.getPosition() / push_angle)); + ctrl_trigger_.setCommand( + push_angle * std::floor(ctrl_trigger_.joint_.getPosition() + 0.01 + config_.exit_push_threshold) / push_angle); else if (enter_ready_) { ctrl_trigger_.setCommand(push_angle * std::floor((ctrl_trigger_.joint_.getPosition() + 0.01) / push_angle));