diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 956dc88c..2afb7a5c 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -323,8 +323,10 @@ 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() + 0.01 + config_.exit_push_threshold) / push_angle); + 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));