Skip to content

Commit

Permalink
Modify some code specifications.
Browse files Browse the repository at this point in the history
  • Loading branch information
liyixin135 committed Mar 3, 2024
1 parent 471fc79 commit 67d55fe
Showing 1 changed file with 4 additions and 4 deletions.
8 changes: 4 additions & 4 deletions rm_shooter_controllers/src/standard.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -177,25 +177,25 @@ void Controller::ready(const ros::Duration& period)

void Controller::push(const ros::Time& time, const ros::Duration& period)
{
bool friction_rotate_state = true;
if (state_changed_)
{ // on enter
state_changed_ = false;
ROS_INFO("[Shooter] Enter PUSH");
}
bool wheel_speed_ready = true;
for (auto& ctrl_friction_l : ctrls_friction_l_)
{
if (ctrl_friction_l->joint_.getVelocity() < push_wheel_speed_threshold_ * ctrl_friction_l->command_ ||
ctrl_friction_l->joint_.getVelocity() <= M_PI)
friction_rotate_state = false;
wheel_speed_ready = false;
}
for (auto& ctrl_friction_r : ctrls_friction_r_)
{
if (ctrl_friction_r->joint_.getVelocity() > push_wheel_speed_threshold_ * ctrl_friction_r->command_ ||
ctrl_friction_r->joint_.getVelocity() >= -M_PI)
friction_rotate_state = false;
wheel_speed_ready = false;
}
if ((cmd_.wheel_speed == 0. || friction_rotate_state) && (time - last_shoot_time_).toSec() >= 1. / cmd_.hz)
if ((cmd_.wheel_speed == 0. || wheel_speed_ready) && (time - last_shoot_time_).toSec() >= 1. / cmd_.hz)
{ // Time to shoot!!!
if (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) <
config_.forward_push_threshold)
Expand Down

0 comments on commit 67d55fe

Please sign in to comment.