diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 69bd2545..93629a44 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -95,6 +95,7 @@ class Controller : public controller_interface::MultiInterfaceController("command", 1, &Controller::commandCB, this); local_heat_state_pub_.reset(new realtime_tools::RealtimePublisher( @@ -184,6 +186,7 @@ void Controller::ready(const ros::Duration& period) void Controller::push(const ros::Time& time, const ros::Duration& period) { + static int friction_block_count = 0; if (state_changed_) { // on enter state_changed_ = false; @@ -240,7 +243,20 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) maybe_block_ = false; } else + { ROS_DEBUG("[Shooter] Wait for friction wheel"); + double command = (friction_block_count <= anti_friction_block_duty_cycle_ * 1000) ? anti_friction_block_vel_ : 0; + for (auto& ctrl_friction_l : ctrls_friction_l_) + { + ctrl_friction_l->setCommand(command); + } + for (auto& ctrl_friction_r : ctrls_friction_r_) + { + ctrl_friction_r->setCommand(command); + } + friction_block_count = (friction_block_count + 1) % 1000; + } +} } void Controller::block(const ros::Time& time, const ros::Duration& period)