diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 70423055..ccf2e7ea 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -310,8 +310,11 @@ void Controller::setSpeed(const rm_msgs::ShootCmd& cmd) void Controller::normalize() { double push_angle = 2. * M_PI / static_cast(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() / 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) @@ -324,7 +327,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_) { @@ -356,6 +358,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");