HFUT_Firmament_EC
/
basic_framework
12
0
Fork 0
Code Issues 1 Pull Requests Packages Projects Releases Wiki Activity

英雄火控完成待测试

This commit is contained in:
sph 2024-03-09 23:13:39 +08:00
parent 2ee9f9e6d5
commit 2aac4702b9
2 changed files with 136 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
application/cmd/robot_cmd.c
View File

@ -176,17 +176,25 @@ static void RemoteControlSet()
no_find_cnt = 0; no_find_cnt = 0;
auto_aim_flag = 1; 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.yaw = -trajectory_cal.cmd_yaw * 180 / PI;
gimbal_cmd_send.pitch = -trajectory_cal.cmd_pitch * 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度 if (yaw_err <= 3) //3度
aim_select.suggest_fire = 1; aim_select.suggest_fire = 1;
else else
aim_select.suggest_fire = 0; 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;
} }
} }
// 云台软件限位 // 云台软件限位

147
modules/auto_aim/auto_aim.c
View File

@ -18,6 +18,9 @@ void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_
int use_1 = 1; int use_1 = 1;
int i = 0; int i = 0;
int idx = 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) { if (aim_sel->target_state.armor_num == 2) {
for (i = 0; i < 2; i++) { 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; 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 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); // float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[1].yaw);
if (temp_yaw_diff < yaw_diff_min) { // if (temp_yaw_diff < yaw_diff_min) {
yaw_diff_min = temp_yaw_diff; // 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; idx = 1;
} }
} else if (aim_sel->target_state.armor_num == 3)//前哨站 } else if (aim_sel->target_state.armor_num == 3)//前哨站
{ {
for (i = 0; i < 3; i++) { 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; use_1 = !use_1;
} }
//计算枪管到目标装甲板yaw最小的那个装甲板 // //计算枪管到目标装甲板yaw最小的那个装甲板
float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].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++) { for (i = 1; i < 3; i++) {
float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw); if (distance[i] < distance[label_first]) {
if (temp_yaw_diff < yaw_diff_min) { label_second = label_first;
yaw_diff_min = temp_yaw_diff; label_first = i;
idx = 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 { } else {
for (i = 0; i < 4; i++) { for (i = 0; i < 4; i++) {
float tmp_yaw = aim_sel->target_state.yaw + i * PI / 2.0; 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; use_1 = !use_1;
} }
//计算枪管到目标装甲板yaw最小的那个装甲板 // //计算枪管到目标装甲板yaw最小的那个装甲板
float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].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++) { for (i = 1; i < 4; i++) {
float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw); if (distance[i] < distance[label_first]) {
if (temp_yaw_diff < yaw_diff_min) { label_second = label_first;
yaw_diff_min = temp_yaw_diff; label_first = i;
idx = 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; float r = use_1 ? aim_sel->target_state.r2 : aim_sel->target_state.r1;
aim_sel->aim_point[2] = aim_sel->armor_pose[idx].z + aim_sel->target_state.vz * aim_sel->delay_time; 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);
} }
/** /**
@ -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) { 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_type = receive_packet->id;
aim_sel->target_state.armor_num = receive_packet->armors_num; aim_sel->target_state.armor_num = receive_packet->armors_num;