Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Achieved double stage friction wheel shooting #148

Closed
wants to merge 4 commits into from
Closed

Achieved double stage friction wheel shooting #148

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
7ff5328
Achieved double stage friction wheel shooting.
liyixin135 Nov 23, 2023
d4f35b5
Remove info.
liyixin135 Nov 23, 2023
1534450
Simplify code.
liyixin135 Nov 25, 2023
6c01846
Modify something wrong.
liyixin135 Nov 27, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 4 additions & 2 deletions rm_shooter_controllers/include/rm_shooter_controllers/standard.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -83,10 +83,12 @@ class Controller : public controller_interface::MultiInterfaceController<hardwar
void reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint32_t /*level*/);

hardware_interface::EffortJointInterface* effort_joint_interface_{};
effort_controllers::JointVelocityController ctrl_friction_l_, ctrl_friction_r_;
effort_controllers::JointVelocityController ctrl_friction_l_, ctrl_friction_r_, ctrl_friction_l_b_,
ctrl_friction_r_b_;
effort_controllers::JointPositionController ctrl_trigger_;
int push_per_rotation_{};
double push_wheel_speed_threshold_{};
double offset_, push_wheel_speed_threshold_{};
bool is_double_stage_ = false;
bool dynamic_reconfig_initialized_ = false;
bool state_changed_ = false;
bool maybe_block_ = false;
Expand Down
40 changes: 36 additions & 4 deletions rm_shooter_controllers/src/standard.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,17 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro
ros::NodeHandle nh_friction_r = ros::NodeHandle(controller_nh, "friction_right");
ros::NodeHandle nh_trigger = ros::NodeHandle(controller_nh, "trigger");
effort_joint_interface_ = robot_hw->get<hardware_interface::EffortJointInterface>();
if (controller_nh.getParam("offset", offset_))
{
is_double_stage_ = true;
ros::NodeHandle nh_friction_l_b = ros::NodeHandle(controller_nh, "friction_left_back");
ros::NodeHandle nh_friction_r_b = ros::NodeHandle(controller_nh, "friction_right_back");
if (!(ctrl_friction_l_b_.init(effort_joint_interface_, nh_friction_l_b) &&
ctrl_friction_r_b_.init(effort_joint_interface_, nh_friction_r_b)))
{
return false;
}
}
return ctrl_friction_l_.init(effort_joint_interface_, nh_friction_l) &&
ctrl_friction_r_.init(effort_joint_interface_, nh_friction_r) &&
ctrl_trigger_.init(effort_joint_interface_, nh_trigger);
Expand Down Expand Up @@ -124,6 +135,11 @@ void Controller::update(const ros::Time& time, const ros::Duration& period)
ctrl_friction_l_.update(time, period);
ctrl_friction_r_.update(time, period);
ctrl_trigger_.update(time, period);
if (is_double_stage_)
{
ctrl_friction_r_b_.update(time, period);
ctrl_friction_l_b_.update(time, period);
}
}

void Controller::stop(const ros::Time& time, const ros::Duration& period)
Expand All @@ -135,6 +151,11 @@ void Controller::stop(const ros::Time& time, const ros::Duration& period)

ctrl_friction_l_.setCommand(0.);
ctrl_friction_r_.setCommand(0.);
if (is_double_stage_)
{
ctrl_friction_r_b_.setCommand(0.);
ctrl_friction_l_b_.setCommand(0.);
}
ctrl_trigger_.setCommand(ctrl_trigger_.joint_.getPosition());
}
}
Expand All @@ -158,10 +179,16 @@ void Controller::push(const ros::Time& time, const ros::Duration& period)
ROS_INFO("[Shooter] Enter PUSH");
}
if ((cmd_.wheel_speed == 0. ||
(ctrl_friction_l_.joint_.getVelocity() >= push_wheel_speed_threshold_ * ctrl_friction_l_.command_ &&
ctrl_friction_l_.joint_.getVelocity() > M_PI &&
ctrl_friction_r_.joint_.getVelocity() <= push_wheel_speed_threshold_ * ctrl_friction_r_.command_ &&
ctrl_friction_r_.joint_.getVelocity() < -M_PI)) &&
((ctrl_friction_l_.joint_.getVelocity() >= push_wheel_speed_threshold_ * ctrl_friction_l_.command_ &&
ctrl_friction_l_.joint_.getVelocity() > M_PI &&
ctrl_friction_r_.joint_.getVelocity() <= push_wheel_speed_threshold_ * ctrl_friction_r_.command_ &&
ctrl_friction_r_.joint_.getVelocity() < -M_PI) &&
(!is_double_stage_ ||
(is_double_stage_ &&
ctrl_friction_l_b_.joint_.getVelocity() >= push_wheel_speed_threshold_ * ctrl_friction_l_b_.command_ &&
ctrl_friction_l_b_.joint_.getVelocity() > M_PI &&
ctrl_friction_r_b_.joint_.getVelocity() <= push_wheel_speed_threshold_ * ctrl_friction_r_b_.command_ &&
ctrl_friction_r_b_.joint_.getVelocity() < -M_PI)))) &&
(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) <
Expand Down Expand Up @@ -221,6 +248,11 @@ void Controller::setSpeed(const rm_msgs::ShootCmd& cmd)
{
ctrl_friction_l_.setCommand(cmd_.wheel_speed + config_.extra_wheel_speed);
ctrl_friction_r_.setCommand(-cmd_.wheel_speed - config_.extra_wheel_speed);
if (is_double_stage_ && cmd.wheel_speed != 0)
{
ctrl_friction_l_b_.setCommand(cmd_.wheel_speed + config_.extra_wheel_speed - offset_);
ctrl_friction_r_b_.setCommand(-cmd_.wheel_speed - config_.extra_wheel_speed + offset_);
}
}

void Controller::normalize()
Expand Down
Loading