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

Change constant-speed gyro to dynamic-speed gyro. #124

Merged
merged 5 commits into from
Dec 7, 2024
Merged

Change constant-speed gyro to dynamic-speed gyro. #124

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
5 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
bdc18f6
Standardlize some code.
Rayy0527 Dec 6, 2024
c4208e0
Optimize some code.
Rayy0527 Dec 7, 2024
a189adb
Standardlize some code.
Rayy0527 Dec 7, 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);
Rayy0527 marked this conversation as resolved.
Show resolved Hide resolved
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. };
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

同一个功能的四个变量要放在一行里面写，不要分几行

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)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

在函数里不会改变的变量，加上const。

{
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)
Rayy0527 marked this conversation as resolved.
Show resolved Hide resolved
{
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_);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

这里不需要改吧

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_));
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

这里不需要改吧

}
}

Expand Down Expand Up @@ -132,6 +155,22 @@ void ChassisGimbalShooterCoverManual::sendCommand(const ros::Time& time)
}
ChassisGimbalShooterManual::sendCommand(time);
cover_command_sender_->sendCommand(time);
Rayy0527 marked this conversation as resolved.
Show resolved Hide resolved
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);
Rayy0527 marked this conversation as resolved.
Show resolved Hide resolved
}
}

void ChassisGimbalShooterCoverManual::rightSwitchDownRise()
Expand Down
Loading