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

Dynamic-speed gyro #124

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Dynamic-speed gyro #124

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
9b081f5
Add sin speed gyro.
Rayy0527 Nov 24, 2024
ea5e1cc
Maintain Rc wheel function.
Rayy0527 Dec 1, 2024
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
5 changes: 5 additions & 0 deletions include/rm_manual/chassis_gimbal_shooter_cover_manual.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual

protected:
void changeSpeedMode(SpeedMode speed_mode);
double getDynamicScale(double base_scale, double amplitude, double period, double phase);
void changeGyroSpeedMode(SpeedMode speed_mode);
void updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) override;
void checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) override;
Expand Down Expand Up @@ -52,6 +53,10 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual
double low_speed_scale_{}, normal_speed_scale_{};
double exit_buff_mode_duration_{};
double gyro_speed_limit_{};
double sin_gyro_base_scale_{ 1. };
double sin_gyro_amplitude_{ 0. };
double sin_gyro_period_{ 1. };
double sin_gyro_phase_{ 0. };
rm_common::SwitchDetectionCaller* switch_buff_srv_{};
rm_common::SwitchDetectionCaller* switch_buff_type_srv_{};
rm_common::SwitchDetectionCaller* switch_exposure_srv_{};
Expand Down
45 changes: 42 additions & 3 deletions src/chassis_gimbal_shooter_cover_manual.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,11 @@ ChassisGimbalShooterCoverManual::ChassisGimbalShooterCoverManual(ros::NodeHandle
low_speed_scale_ = chassis_nh.param("low_speed_scale", 0.30);
nh.param("exit_buff_mode_duration", exit_buff_mode_duration_, 0.5);
nh.param("gyro_speed_limit", gyro_speed_limit_, 6.0);
ros::NodeHandle vel_nh(nh, "vel");
sin_gyro_base_scale_ = vel_nh.param("sin_gyro_base_scale", 1.0);
sin_gyro_amplitude_ = vel_nh.param("sin_gyro_amplitude", 0.0);
sin_gyro_period_ = vel_nh.param("sin_gyro_period", 1.0);
sin_gyro_phase_ = vel_nh.param("sin_gyro_phase", 0.0);

ctrl_z_event_.setEdge(boost::bind(&ChassisGimbalShooterCoverManual::ctrlZPress, this),
boost::bind(&ChassisGimbalShooterCoverManual::ctrlZRelease, this));
Expand All @@ -42,7 +47,22 @@ void ChassisGimbalShooterCoverManual::changeSpeedMode(SpeedMode speed_mode)
speed_change_scale_ = normal_speed_scale_;
}
}

double ChassisGimbalShooterCoverManual::getDynamicScale(double base_scale, double amplitude, double period, double phase)
{
ros::Time current_time = ros::Time::now();
double t = current_time.toSec();
double f = 2 * M_PI / period;
double dynamic_scale = base_scale + amplitude * sin(f * t + phase);
if (dynamic_scale < 0.0)
{
dynamic_scale = 0.0;
}
else if (dynamic_scale > 1.0)
{
dynamic_scale = 1.0;
}
return dynamic_scale;
}
void ChassisGimbalShooterCoverManual::changeGyroSpeedMode(SpeedMode speed_mode)
{
if (speed_mode == LOW)
Expand All @@ -55,9 +75,12 @@ void ChassisGimbalShooterCoverManual::changeGyroSpeedMode(SpeedMode speed_mode)
else if (speed_mode == NORMAL)
{
if (x_scale_ != 0.0 || y_scale_ != 0.0)
vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_);
vel_cmd_sender_->setAngularZVel(
getDynamicScale(sin_gyro_base_scale_, sin_gyro_amplitude_, sin_gyro_period_, sin_gyro_phase_) *
gyro_rotate_reduction_);
else
vel_cmd_sender_->setAngularZVel(1.0);
vel_cmd_sender_->setAngularZVel(
getDynamicScale(sin_gyro_base_scale_, sin_gyro_amplitude_, sin_gyro_period_, sin_gyro_phase_));
}
}

Expand Down Expand Up @@ -132,6 +155,22 @@ void ChassisGimbalShooterCoverManual::sendCommand(const ros::Time& time)
}
ChassisGimbalShooterManual::sendCommand(time);
cover_command_sender_->sendCommand(time);
if (state_ == PC && is_gyro_)
{
if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR)
if (x_scale_ != 0.0 || y_scale_ != 0.0)
vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_, gyro_speed_limit_);
else
vel_cmd_sender_->setAngularZVel(1.0, gyro_speed_limit_);
else if (x_scale_ != 0.0 || y_scale_ != 0.0)
vel_cmd_sender_->setAngularZVel(
getDynamicScale(sin_gyro_base_scale_, sin_gyro_amplitude_, sin_gyro_period_, sin_gyro_phase_) *
gyro_rotate_reduction_);
else
vel_cmd_sender_->setAngularZVel(
getDynamicScale(sin_gyro_base_scale_, sin_gyro_amplitude_, sin_gyro_period_, sin_gyro_phase_));
vel_cmd_sender_->sendCommand(time);
}
}

void ChassisGimbalShooterCoverManual::rightSwitchDownRise()
Expand Down