diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 2a2b4f70..bf07962d 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -90,6 +91,7 @@ class Controller : public controller_interface::MultiInterfaceController> wheel_speed_offsets_; std::vector> wheel_speed_directions_; + LowPassFilter* lp_filter_; int push_per_rotation_{}, count_{}; double push_wheel_speed_threshold_{}; double freq_threshold_{}; @@ -102,8 +104,9 @@ class Controller : public controller_interface::MultiInterfaceControllerinput(ctrls_friction_[0][0]->joint_.getVelocity()); + double friction_speed = lp_filter_->output(); + double friction_change_speed = abs(friction_speed) - last_fricition_speed_; + double friction_change_speed_derivative = friction_change_speed - last_friction_change_speed_; if (state_ != STOP) { - if (abs(ctrls_friction_[0][0]->joint_.getVelocity()) - last_wheel_speed_ > config_.wheel_speed_raise_threshold && - wheel_speed_drop_) - { - wheel_speed_raise_ = true; - wheel_speed_drop_ = false; - } - - if (last_wheel_speed_ - abs(ctrls_friction_[0][0]->joint_.getVelocity()) > config_.wheel_speed_drop_threshold && - abs(ctrls_friction_[0][0]->joint_.getVelocity()) > 300. && wheel_speed_raise_) - { - wheel_speed_drop_ = true; - wheel_speed_raise_ = false; + if (friction_change_speed_derivative > 0 && has_shoot_) + has_shoot_ = false; + if (friction_change_speed < -config_.wheel_speed_drop_threshold && !has_shoot_ && + friction_change_speed_derivative < 0) has_shoot_ = true; - } } - double friction_change_vel = abs(ctrls_friction_[0][0]->joint_.getVelocity()) - last_wheel_speed_; - last_wheel_speed_ = abs(ctrls_friction_[0][0]->joint_.getVelocity()); - count_++; - if (has_shoot_last_) - { - has_shoot_ = true; - } - has_shoot_last_ = has_shoot_; - if (count_ == 2) + last_fricition_speed_ = abs(friction_speed); + last_friction_change_speed_ = friction_change_speed; + + if (local_heat_state_pub_->trylock()) { - if (local_heat_state_pub_->trylock()) - { - local_heat_state_pub_->msg_.stamp = time; - local_heat_state_pub_->msg_.has_shoot = has_shoot_; - local_heat_state_pub_->msg_.friction_change_vel = friction_change_vel; - local_heat_state_pub_->unlockAndPublish(); - } - has_shoot_last_ = false; - count_ = 0; + local_heat_state_pub_->msg_.stamp = time; + local_heat_state_pub_->msg_.has_shoot = has_shoot_; + local_heat_state_pub_->msg_.friction_speed = friction_speed; + local_heat_state_pub_->msg_.friction_change_speed = friction_change_speed; + local_heat_state_pub_->msg_.friction_change_speed_derivative = friction_change_speed_derivative; + local_heat_state_pub_->unlockAndPublish(); } - if (has_shoot_) - has_shoot_ = false; } void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint32_t /*level*/) {