From 750a2bc8ebdd3cfd97bb900c63e40d4ac46b6e90 Mon Sep 17 00:00:00 2001
From: 1moule <1961466188@qq.com>
Date: Fri, 26 Jul 2024 19:51:12 +0800
Subject: [PATCH 1/2] Fix normalizr and cmakelist.

---
 rm_chassis_controllers/CMakeLists.txt   | 9 ++++++---
 rm_shooter_controllers/src/standard.cpp | 9 ++++++---
 2 files changed, 12 insertions(+), 6 deletions(-)

diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt
index 08976ce7..ec9391d0 100644
--- a/rm_chassis_controllers/CMakeLists.txt
+++ b/rm_chassis_controllers/CMakeLists.txt
@@ -19,8 +19,9 @@ find_package(catkin REQUIRED
         controller_interface
         effort_controllers
         tf2_geometry_msgs
+        nav_msgs
         angles
-        )
+)
 
 find_package(Eigen3 REQUIRED)
 
@@ -45,6 +46,8 @@ catkin_package(
 
         effort_controllers
         tf2_geometry_msgs
+        nav_msgs
+        angles
         LIBRARIES ${PROJECT_NAME}
 )
 
@@ -66,12 +69,12 @@ add_library(${PROJECT_NAME}
         src/sentry.cpp
         src/swerve.cpp
         src/balance.cpp
-        )
+)
 
 ## Specify libraries to link executable targets against
 target_link_libraries(${PROJECT_NAME}
         ${catkin_LIBRARIES}
-        )
+)
 
 #############
 ## Install ##
diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp
index 70efdcb9..046e4d61 100644
--- a/rm_shooter_controllers/src/standard.cpp
+++ b/rm_shooter_controllers/src/standard.cpp
@@ -274,8 +274,11 @@ void Controller::setSpeed(const rm_msgs::ShootCmd& cmd)
 void Controller::normalize()
 {
   double push_angle = 2. * M_PI / static_cast<double>(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() + 0.01) / push_angle));
+  else
+    ctrl_trigger_.setCommand(
+        push_angle * std::floor((ctrl_trigger_.joint_.getPosition() + 0.01 + config_.exit_push_threshold) / push_angle));
 }
 
 void Controller::judgeBulletShoot(const ros::Time& time, const ros::Duration& period)
@@ -288,7 +291,6 @@ void Controller::judgeBulletShoot(const ros::Time& time, const ros::Duration& pe
       wheel_speed_raise_ = true;
       wheel_speed_drop_ = false;
     }
-
     if (last_wheel_speed_ - abs(ctrls_friction_l_[0]->joint_.getVelocity()) > config_.wheel_speed_drop_threshold &&
         abs(ctrls_friction_l_[0]->joint_.getVelocity()) > 300. && wheel_speed_raise_)
     {
@@ -320,6 +322,7 @@ void Controller::judgeBulletShoot(const ros::Time& time, const ros::Duration& pe
   if (has_shoot_)
     has_shoot_ = false;
 }
+
 void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint32_t /*level*/)
 {
   ROS_INFO("[Shooter] Dynamic params change");

From c7654c56af4d28ada13e73aa5bda7dda9d80edb6 Mon Sep 17 00:00:00 2001
From: liyixin135 <421159734@qq.com>
Date: Sun, 4 Aug 2024 05:10:33 +0800
Subject: [PATCH 2/2] Modify value.

---
 rm_shooter_controllers/src/standard.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp
index 046e4d61..1a88d8c0 100644
--- a/rm_shooter_controllers/src/standard.cpp
+++ b/rm_shooter_controllers/src/standard.cpp
@@ -275,7 +275,7 @@ void Controller::normalize()
 {
   double push_angle = 2. * M_PI / static_cast<double>(push_per_rotation_);
   if (cmd_.hz >= freq_threshold_)
-    ctrl_trigger_.setCommand(push_angle * std::floor((ctrl_trigger_.joint_.getPosition() + 0.01) / push_angle));
+    ctrl_trigger_.setCommand(push_angle * std::floor(ctrl_trigger_.joint_.getPosition() / push_angle));
   else
     ctrl_trigger_.setCommand(
         push_angle * std::floor((ctrl_trigger_.joint_.getPosition() + 0.01 + config_.exit_push_threshold) / push_angle));