From 6f1a15297f153053f09c30bf92b0c080b2d92602 Mon Sep 17 00:00:00 2001
From: Fawo011 <LMLSDSG@hotmail.com>
Date: Sun, 14 Jan 2024 01:53:16 +0800
Subject: [PATCH] Delete the velocity planning of low hz mode

---
 .../include/rm_shooter_controllers/standard.h     |  1 -
 rm_shooter_controllers/src/standard.cpp           | 15 +--------------
 2 files changed, 1 insertion(+), 15 deletions(-)

diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h
index f704572e..d351e106 100644
--- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h
+++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h
@@ -86,7 +86,6 @@ class Controller : public controller_interface::MultiInterfaceController<hardwar
   effort_controllers::JointVelocityController ctrl_friction_l_, ctrl_friction_r_;
   effort_controllers::JointPositionController ctrl_trigger_;
   int push_per_rotation_{};
-  double expect_velocity = 0;
   double push_wheel_speed_threshold_{};
   bool dynamic_reconfig_initialized_ = false;
   bool state_changed_ = false;
diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp
index bcccc8be..a9c9ecb6 100644
--- a/rm_shooter_controllers/src/standard.cpp
+++ b/rm_shooter_controllers/src/standard.cpp
@@ -179,23 +179,11 @@ void Controller::push(const ros::Time& time, const ros::Duration& period)
     }
     else
     {
-      if (ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition() >=
-          (static_cast<double>(push_per_rotation_) / 2))
-      {
-        expect_velocity += std::pow(-1 * cmd_.hz * 2. * M_PI / static_cast<double>(push_per_rotation_), 2) /
-            (2 * (ctrl_trigger_.command_struct_.position_ - 2. * M_PI / static_cast<double>(push_per_rotation_)));
-      }
-      else
-      {
-        expect_velocity -= std::pow(-1 * cmd_.hz * 2. * M_PI / static_cast<double>(push_per_rotation_), 2) /
-            (2 * (ctrl_trigger_.command_struct_.position_ - 2. * M_PI / static_cast<double>(push_per_rotation_)));
-      }
       if (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) <
           config_.forward_push_threshold)
       {
         ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ -
-                                     2. * M_PI / static_cast<double>(push_per_rotation_),
-                                 expect_velocity);
+                                 2. * M_PI / static_cast<double>(push_per_rotation_));
         last_shoot_time_ = time;
       }
     }
@@ -226,7 +214,6 @@ void Controller::push(const ros::Time& time, const ros::Duration& period)
 
 void Controller::block(const ros::Time& time, const ros::Duration& period)
 {
-  expect_velocity = 0;
   if (state_changed_)
   {  // on enter
     state_changed_ = false;