Delete the velocity planning of low hz mode

rm-controls · Jan 13, 2024 · 6f1a152 · 6f1a152
1 parent b0d6774
commit 6f1a152
Show file tree

Hide file tree

Showing 2 changed files with 1 addition and 15 deletions.
diff --git 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
@@ -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;