diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index d3c0848b..1e5a9535 100755 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -12,8 +12,8 @@ gen.add("resistance_coff_qd_18", double_t, 0, "Air resistance divided by mass of gen.add("resistance_coff_qd_30", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) gen.add("g", double_t, 0, "Air resistance divided by mass", 9.8, 9.6, 10.0) gen.add("delay", double_t, 0, "Delay of bullet firing", 0.0, 0, 0.5) -gen.add("track_center_next_delay", double_t, 0, "Delay of shoot next target when in center mode", 0.0, 0.105, 0.5) -gen.add("track_center_second_delay", double_t, 0, "Delay of shoot second target when in center mode", 0.0, 0.105, 0.5) +gen.add("wait_next_armor_delay", double_t, 0, "Delay of shooting next target when in center mode", 0.0, 0.105, 0.5) +gen.add("wait_diagonal_armor_delay", double_t, 0, "Delay of shooting diagonal target when in center mode", 0.0, 0.105, 0.5) gen.add("dt", double_t, 0, "Iteration interval", 0.01, 0.0001, 0.1) gen.add("timeout", double_t, 0, "Flight time exceeded", 2.0, 0.5, 3.0) gen.add("ban_shoot_duration", double_t, 0, "Ban shoot duration while beforehand shooting", 0.0, 0.0, 2.0) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h index 7fb369cd..eb171b8b 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -56,7 +56,7 @@ namespace rm_gimbal_controllers struct Config { double resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18, - resistance_coff_qd_30, g, delay, track_center_next_delay, track_center_second_delay, dt, timeout, + resistance_coff_qd_30, g, delay, wait_next_armor_delay, wait_diagonal_armor_delay, dt, timeout, ban_shoot_duration, gimbal_switch_duration, max_switch_angle, min_switch_angle, min_shoot_beforehand_vel, max_chassis_angular_vel, track_rotate_target_delay, track_move_target_delay; int min_fit_switch_count; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 3e35a517..7deb568c 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -51,8 +51,8 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .resistance_coff_qd_30 = getParam(controller_nh, "resistance_coff_qd_30", 0.), .g = getParam(controller_nh, "g", 0.), .delay = getParam(controller_nh, "delay", 0.), - .track_center_next_delay = getParam(controller_nh, "track_center_next_delay", 0.105), - .track_center_second_delay = getParam(controller_nh, "track_center_second_delay", 0.105), + .wait_next_armor_delay = getParam(controller_nh, "wait_next_armor_delay", 0.105), + .wait_diagonal_armor_delay = getParam(controller_nh, "wait_diagonal_armor_delay", 0.105), .dt = getParam(controller_nh, "dt", 0.), .timeout = getParam(controller_nh, "timeout", 0.), .ban_shoot_duration = getParam(controller_nh, "ban_shoot_duration", 0.0), @@ -186,9 +186,9 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d { target_pos_.x = pos.x - r * cos(atan2(pos.y, pos.x)); target_pos_.y = pos.y - r * sin(atan2(pos.y, pos.x)); - if ((v_yaw > 1.0 && (yaw + v_yaw * (fly_time_ + config_.track_center_next_delay) + + if ((v_yaw > 1.0 && (yaw + v_yaw * (fly_time_ + config_.wait_next_armor_delay) + selected_armor_ * 2 * M_PI / armors_num) > output_yaw_) || - (v_yaw < -1.0 && (yaw + v_yaw * (fly_time_ + config_.track_center_next_delay) + + (v_yaw < -1.0 && (yaw + v_yaw * (fly_time_ + config_.wait_next_armor_delay) + selected_armor_ * 2 * M_PI / armors_num) < output_yaw_)) selected_armor_ = v_yaw > 0. ? -2 : 2; if (selected_armor_ % 2 == 0) @@ -330,7 +330,7 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec if (track_target_) delay = 0.; else - delay = selected_armor_ % 2 == 0 ? config_.track_center_second_delay : config_.track_center_next_delay; + delay = selected_armor_ % 2 == 0 ? config_.wait_diagonal_armor_delay : config_.wait_next_armor_delay; double r, z; if (selected_armor_ % 2 == 0) { @@ -412,8 +412,8 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.resistance_coff_qd_30 = init_config.resistance_coff_qd_30; config.g = init_config.g; config.delay = init_config.delay; - config.track_center_next_delay = init_config.track_center_next_delay; - config.track_center_second_delay = init_config.track_center_second_delay; + config.wait_next_armor_delay = init_config.wait_next_armor_delay; + config.wait_diagonal_armor_delay = init_config.wait_diagonal_armor_delay; config.dt = init_config.dt; config.timeout = init_config.timeout; config.ban_shoot_duration = init_config.ban_shoot_duration; @@ -434,8 +434,8 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .resistance_coff_qd_30 = config.resistance_coff_qd_30, .g = config.g, .delay = config.delay, - .track_center_next_delay = config.track_center_next_delay, - .track_center_second_delay = config.track_center_second_delay, + .wait_next_armor_delay = config.wait_next_armor_delay, + .wait_diagonal_armor_delay = config.wait_diagonal_armor_delay, .dt = config.dt, .timeout = config.timeout, .ban_shoot_duration = config.ban_shoot_duration,