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<hardwar
   double freq_threshold_{};
   bool dynamic_reconfig_initialized_ = false;
   bool state_changed_ = false;
+  bool enter_ready_ = false;
   bool maybe_block_ = false;
   double friction_block_effort_{}, friction_block_vel_{};
   double anti_friction_block_duty_cycle_{}, anti_friction_block_vel_{};
diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp
index 5ea06cd6..9f6823fd 100644
--- a/rm_shooter_controllers/src/standard.cpp
+++ b/rm_shooter_controllers/src/standard.cpp
@@ -119,6 +119,8 @@ void Controller::update(const ros::Time& time, const ros::Duration& period)
                 config_.exit_push_threshold ||
             cmd_.hz >= freq_threshold_)))
       {
+        if (state_ == STOP && cmd_.mode == READY)
+          enter_ready_ = true;
         state_ = cmd_.mode;
         state_changed_ = true;
       }
@@ -306,8 +308,16 @@ 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 + 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));
+    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)