Skip to content

Note #194

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

Closed
xuncheng-001 wants to merge 2 commits into from
Closed

Note #194

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
50fc9d8
My learning note.
YoujianWu Jan 12, 2025
256c8f7
Merge remote-tracking branch 'origin/note' into note
xuncheng-001 Apr 24, 2025
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
15 changes: 14 additions & 1 deletion rm_gimbal_controllers/src/bullet_solver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -102,6 +102,7 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh)
double BulletSolver::getResistanceCoefficient(double bullet_speed) const
{
// bullet_speed have 5 value:10,15,16,18,30
//根据子弹的速度，选择不同的阻力系数，返回值是阻力系数
double resistance_coff;
if (bullet_speed < 12.5)
resistance_coff = config_.resistance_coff_qd_10;
Expand All @@ -119,12 +120,14 @@ double BulletSolver::getResistanceCoefficient(double bullet_speed) const
bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw,
double v_yaw, double r1, double r2, double dz, int armors_num, double chassis_angular_vel_z)
{
//在缓冲区读取数据，然后获取空气阻力系数
config_ = *config_rt_buffer_.readFromRT();
bullet_speed_ = bullet_speed;
resistance_coff_ = getResistanceCoefficient(bullet_speed_) != 0 ? getResistanceCoefficient(bullet_speed_) : 0.001;

double temp_z = pos.z;
double target_rho = std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2));
//在这里计算目标的yaw和pitch，然后计算飞行时间
output_yaw_ = std::atan2(pos.y, pos.x);
output_pitch_ = std::atan2(temp_z, std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)));
double rough_fly_time =
Expand All @@ -134,29 +137,38 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
double z = pos.z;
double max_switch_angle = config_.max_switch_angle / 180 * M_PI;
double min_switch_angle = config_.min_switch_angle / 180 * M_PI;
//判断是否跟踪移动的目标（装甲板）,速度超过最大跟踪速度，就不跟踪移动的目标
track_target_ = std::abs(v_yaw) < max_track_target_vel_;
double switch_armor_angle =
track_target_ ?
//跟踪目标时候的计算
(acos(r / target_rho) - max_switch_angle +
(-acos(r / target_rho) + (max_switch_angle + min_switch_angle)) * std::abs(v_yaw) / max_track_target_vel_) *
(1 - std::abs(chassis_angular_vel_z) / config_.max_chassis_angular_vel) +
min_switch_angle * std::abs(chassis_angular_vel_z) / config_.max_chassis_angular_vel :
min_switch_angle;
//不跟踪目标的话直接使用最小切换速度
min_switch_angle;
//这里的yaw+v_yaw*rough_fly_time 是预测的角度，与最大和最小允许角度进行比较，
if (((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) ||
(((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) &&
std::abs(v_yaw) >= 1.0)
{
count_++;
//判断是否切换了跟踪目标
if (identified_target_change_)
{
//如果是就重新记数
count_ = 0;
identified_target_change_ = false;
}
if (count_ >= config_.min_fit_switch_count)
{
//计数超过一定次数，就就记录时间
if (count_ == config_.min_fit_switch_count)
switch_armor_time_ = ros::Time::now();
//通过转动的方向来判断是要选上一个装甲板还是下一个
selected_armor_ = v_yaw > 0. ? -1 : 1;
//这里应该是四号装甲板有特殊的半径和高度
r = armors_num == 4 ? r2 : r1;
z = armors_num == 4 ? pos.z + dz : pos.z;
}
Expand All @@ -169,6 +181,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
output_yaw_ - switch_armor_angle) &&
v_yaw < 0.)) &&
track_target_;
//如果是跟踪模式，
if (track_target_)
yaw += v_yaw * config_.track_rotate_target_delay;
pos.x += vel.x * config_.track_move_target_delay;
Expand Down
2 changes: 1 addition & 1 deletion rm_shooter_controllers/src/standard.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro
.extra_wheel_speed = getParam(controller_nh, "extra_wheel_speed", 0.),
.wheel_speed_drop_threshold = getParam(controller_nh, "wheel_speed_drop_threshold", 10.),
.wheel_speed_raise_threshold = getParam(controller_nh, "wheel_speed_raise_threshold", 3.1) };
//初始化实时配置缓冲区，将初始配置参数载入缓冲区
config_rt_buffer.initRT(config_);
push_per_rotation_ = getParam(controller_nh, "push_per_rotation", 0);
push_wheel_speed_threshold_ = getParam(controller_nh, "push_wheel_speed_threshold", 0.);
Expand All @@ -63,7 +64,6 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro
anti_friction_block_vel_ = getParam(controller_nh, "anti_friction_block_vel", 810.0);
friction_block_effort_ = getParam(controller_nh, "friction_block_effort", 0.2);
friction_block_vel_ = getParam(controller_nh, "friction_block_vel", 1.0);

cmd_subscriber_ = controller_nh.subscribe<rm_msgs::ShootCmd>("command", 1, &Controller::commandCB, this);
local_heat_state_pub_.reset(new realtime_tools::RealtimePublisher<rm_msgs::LocalHeatState>(
controller_nh, "/local_heat_state/shooter_state", 10));
Expand Down
Loading