From 2aac4702b97645dffc1ab13bdcec572f109e7f93 Mon Sep 17 00:00:00 2001 From: sph <1527550984@qq.com> Date: Sat, 9 Mar 2024 23:13:39 +0800 Subject: [PATCH] =?UTF-8?q?=E8=8B=B1=E9=9B=84=E7=81=AB=E6=8E=A7=E5=AE=8C?= =?UTF-8?q?=E6=88=90=E5=BE=85=E6=B5=8B=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/cmd/robot_cmd.c | 12 ++- modules/auto_aim/auto_aim.c | 149 ++++++++++++++++++++++++++++++------ 2 files changed, 136 insertions(+), 25 deletions(-) diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 1493db1..49ab7e0 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -176,17 +176,25 @@ static void RemoteControlSet() no_find_cnt = 0; auto_aim_flag = 1; - auto_aim(&aim_select, &trajectory_cal, vision_recv_data); + int target_index = auto_aim(&aim_select, &trajectory_cal, vision_recv_data); gimbal_cmd_send.yaw = -trajectory_cal.cmd_yaw * 180 / PI; gimbal_cmd_send.pitch = -trajectory_cal.cmd_pitch * 180 / PI; - float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw); + float target_yaw = atan2f(aim_select.armor_pose[target_index].x, + aim_select.armor_pose[target_index].y); + float yaw_err = fabsf(target_yaw - gimbal_fetch_data.gimbal_imu_data.Yaw); if (yaw_err <= 3) //3度 aim_select.suggest_fire = 1; else aim_select.suggest_fire = 0; + +// float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw); +// if (yaw_err <= 3) //3度 +// aim_select.suggest_fire = 1; +// else +// aim_select.suggest_fire = 0; } } // 云台软件限位 diff --git a/modules/auto_aim/auto_aim.c b/modules/auto_aim/auto_aim.c index 82d08d3..45d8bad 100644 --- a/modules/auto_aim/auto_aim.c +++ b/modules/auto_aim/auto_aim.c @@ -18,6 +18,9 @@ void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_ int use_1 = 1; int i = 0; int idx = 0; // 选择的装甲板 + + float center_length = sqrtf(powf(aim_sel->target_state.x, 2) + powf(aim_sel->target_state.y, 2)); + // 为平衡步兵 if (aim_sel->target_state.armor_num == 2) { for (i = 0; i < 2; i++) { @@ -30,14 +33,22 @@ void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_ aim_sel->armor_pose[i].yaw = aim_sel->target_state.yaw + i * PI; } - float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw); - - //因为是平衡步兵 只需判断两块装甲板即可 - float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[1].yaw); - if (temp_yaw_diff < yaw_diff_min) { - yaw_diff_min = temp_yaw_diff; +// float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw); +// +// //因为是平衡步兵 只需判断两块装甲板即可 +// float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[1].yaw); +// if (temp_yaw_diff < yaw_diff_min) { +// yaw_diff_min = temp_yaw_diff; +// idx = 1; +// } + // 平衡步兵选择两块装甲板中较近的装甲板 + float distance_min = powf(aim_sel->armor_pose[0].x, 2) + powf(aim_sel->armor_pose[0].y, 2); + float distance_temp = powf(aim_sel->armor_pose[1].x, 2) + powf(aim_sel->armor_pose[1].y, 2); + if (distance_temp < distance_min) { + distance_min = distance_temp; idx = 1; } + } else if (aim_sel->target_state.armor_num == 3)//前哨站 { for (i = 0; i < 3; i++) { @@ -55,15 +66,70 @@ void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_ use_1 = !use_1; } - //计算枪管到目标装甲板yaw最小的那个装甲板 - float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw); +// //计算枪管到目标装甲板yaw最小的那个装甲板 +// float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw); +// for (i = 1; i < 3; i++) { +// float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw); +// if (temp_yaw_diff < yaw_diff_min) { +// yaw_diff_min = temp_yaw_diff; +// idx = i; +// } +// } + // 选择两块较近的装甲板，再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板 +// float distance_temp0 = powf(aim_sel->armor_pose[0].x, 2) + powf(aim_sel->armor_pose[0].y, 2); +// float distance_temp1 = powf(aim_sel->armor_pose[1].x, 2) + powf(aim_sel->armor_pose[1].y, 2); +// float distance_temp2 = powf(aim_sel->armor_pose[2].x, 2) + powf(aim_sel->armor_pose[2].y, 2); + +// int label_first = 0; +// int label_second = 1; +// if (distance_temp0 > distance_temp1) { +// label_first = 1; +// if (distance_temp0 > distance_temp2) { +// label_second = 2; +// } else label_second = 0; +// } else { +// label_first = 0; +// if (distance_temp1 > distance_temp2) { +// label_second = 2; +// } else label_second = 1; +// } + + // 选择两块较近的装甲板 + float distance[3]; + for (i = 0; i < 3; i++) { + distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2); + } + + int label_first = 0; + int label_second = 0; + for (i = 1; i < 3; i++) { - float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw); - if (temp_yaw_diff < yaw_diff_min) { - yaw_diff_min = temp_yaw_diff; - idx = i; + if (distance[i] < distance[label_first]) { + label_second = label_first; + label_first = i; + } else if (distance[i] < distance[label_second]) { + label_second = i; + } else if (distance[i] < distance[label_second] && distance[i] < distance[label_first]) { + label_second = i; } } + + //再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板 + + float cos_theta_first = (aim_sel->armor_pose[label_first].x * aim_sel->target_state.x + + aim_sel->armor_pose[label_first].y * aim_sel->target_state.y) / + (sqrtf(powf(aim_sel->armor_pose[label_first].x, 2) + + powf(aim_sel->armor_pose[label_first].y, 2)) * center_length); + float cos_theta_second = (aim_sel->armor_pose[label_second].x * aim_sel->target_state.x + + aim_sel->armor_pose[label_second].y * aim_sel->target_state.y) / + (sqrtf(powf(aim_sel->armor_pose[label_second].x, 2) + + powf(aim_sel->armor_pose[label_second].y, 2)) * center_length); + + if (cos_theta_first > cos_theta_second) + idx = label_first; + else + idx = label_second; + } else { for (i = 0; i < 4; i++) { float tmp_yaw = aim_sel->target_state.yaw + i * PI / 2.0; @@ -77,19 +143,56 @@ void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_ use_1 = !use_1; } - //计算枪管到目标装甲板yaw最小的那个装甲板 - float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw); +// //计算枪管到目标装甲板yaw最小的那个装甲板 +// float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw); +// for (i = 1; i < 4; i++) { +// float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw); +// if (temp_yaw_diff < yaw_diff_min) { +// yaw_diff_min = temp_yaw_diff; +// idx = i; +// } +// } + + // 选择两块较近的装甲板 + float distance[4]; + for (i = 0; i < 4; i++) { + distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2); + } + + int label_first = 0; + int label_second = 0; + for (i = 1; i < 4; i++) { - float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw); - if (temp_yaw_diff < yaw_diff_min) { - yaw_diff_min = temp_yaw_diff; - idx = i; + if (distance[i] < distance[label_first]) { + label_second = label_first; + label_first = i; + } else if (distance[i] < distance[label_second]) { + label_second = i; + } else if (distance[i] < distance[label_second] && distance[i] < distance[label_first]) { + label_second = i; } } + + //再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板 + float cos_theta_first = (aim_sel->armor_pose[label_first].x * aim_sel->target_state.x + + aim_sel->armor_pose[label_first].y * aim_sel->target_state.y) / + (sqrtf(powf(aim_sel->armor_pose[label_first].x, 2) + + powf(aim_sel->armor_pose[label_first].y, 2)) * center_length); + float cos_theta_second = (aim_sel->armor_pose[label_second].x * aim_sel->target_state.x + + aim_sel->armor_pose[label_second].y * aim_sel->target_state.y) / + (sqrtf(powf(aim_sel->armor_pose[label_second].x, 2) + + powf(aim_sel->armor_pose[label_second].y, 2)) * center_length); + + if (cos_theta_first > cos_theta_second) + idx = label_first; + else + idx = label_second; } - aim_sel->aim_point[0] = aim_sel->armor_pose[idx].x + aim_sel->target_state.vx * aim_sel->delay_time; - aim_sel->aim_point[1] = aim_sel->armor_pose[idx].y + aim_sel->target_state.vy * aim_sel->delay_time; - aim_sel->aim_point[2] = aim_sel->armor_pose[idx].z + aim_sel->target_state.vz * aim_sel->delay_time; + + float r = use_1 ? aim_sel->target_state.r2 : aim_sel->target_state.r1; + aim_sel->aim_point[0] = aim_sel->armor_pose[idx].x * ((center_length - r)/center_length); + aim_sel->aim_point[1] = aim_sel->armor_pose[idx].y + ((center_length - r)/center_length); + aim_sel->aim_point[2] = aim_sel->armor_pose[idx].z + ((center_length - r)/center_length); } /** @@ -113,7 +216,7 @@ float get_fly_time(float x, float vx, float v_x0) { } /** - * @brief 解算期望云台角度（考虑子弹飞行时间） + * @brief 解算期望云台角度（考虑子弹飞行时间） * @author SJQ * @param[in] trajectory_cal:弹道解算结构体 * @retval 返回空 @@ -158,7 +261,7 @@ void get_cmd_angle(Trajectory_Type_t *trajectory_cal) { } void auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, RecievePacket_t *receive_packet) { - trajectory_cal->extra_delay_time = 0.025;//0.025 + trajectory_cal->extra_delay_time = 0.035;//0.025 aim_sel->target_state.armor_type = receive_packet->id; aim_sel->target_state.armor_num = receive_packet->armors_num;