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

修改训练赛的问题

This commit is contained in:
zcj 2024-03-27 01:07:52 +08:00
parent b2687cafe3
commit 8de2e62164
3 changed files with 37 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
application/cmd/robot_cmd.c
View File

@ -28,7 +28,7 @@ static Publisher_t *chassis_cmd_pub; // 底盘控制消息发布者
static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者 static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者
#endif // ONE_BOARD #endif // ONE_BOARD
static float temp_yaw; //for test static float temp_yaw; //for test
static float temp_index; //for test static int temp_index; //for test
static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关 static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关
static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等 static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等
@ -36,12 +36,12 @@ static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反
static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回 static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回
//static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回 //static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回
//static Vision_Send_s vision_send_data; // 视觉发送数据 //static Vision_Send_s vision_send_data; // 视觉发送数据
static RecievePacket_t *vision_recv_data; // 视觉接收数据指针,初始化时返回 RecievePacket_t *vision_recv_data; // 视觉接收数据指针,初始化时返回
static SendPacket_t vision_send_data; // 视觉发送数据 SendPacket_t vision_send_data; // 视觉发送数据
//自瞄相关信息 //自瞄相关信息
static Trajectory_Type_t trajectory_cal; Trajectory_Type_t trajectory_cal;
static Aim_Select_Type_t aim_select; Aim_Select_Type_t aim_select;
static uint32_t no_find_cnt; // 未发现目标计数 static uint32_t no_find_cnt; // 未发现目标计数
static uint8_t auto_aim_flag = 0; //辅助瞄准标志位 视野内有目标开启 目标丢失关闭 static uint8_t auto_aim_flag = 0; //辅助瞄准标志位 视野内有目标开启 目标丢失关闭
@ -58,7 +58,7 @@ static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息
static Robot_Status_e robot_state; // 机器人整体工作状态 static Robot_Status_e robot_state; // 机器人整体工作状态
static referee_info_t* referee_data; // 用于获取裁判系统的数据 static referee_info_t* referee_data; // 用于获取裁判系统的数据
extern float horizontal_angle; extern float horizontal_angle;
static int target_index = -1; //static int target_index = -1;
void RobotCMDInit() void RobotCMDInit()
{ {
@ -169,6 +169,7 @@ static void auto_aim_mode()
no_find_cnt = 0; no_find_cnt = 0;
auto_aim_flag = 1; auto_aim_flag = 1;
int target_index = -1;
target_index = auto_aim(&aim_select, &trajectory_cal, vision_recv_data); target_index = auto_aim(&aim_select, &trajectory_cal, vision_recv_data);
temp_index = target_index; temp_index = target_index;
VisionSetAim(aim_select.aim_point[0],aim_select.aim_point[1],aim_select.aim_point[2]); VisionSetAim(aim_select.aim_point[0],aim_select.aim_point[1],aim_select.aim_point[2]);

84
modules/auto_aim/auto_aim.c
View File

@ -17,7 +17,7 @@ int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_c
//计算四块装甲板的位置 //计算四块装甲板的位置
int use_1 = 1; int use_1 = 1;
int i = 0; int i = 0;
int idx = 0; // 选择的装甲板 int idx = -1; // 选择的装甲板
// 为平衡步兵 // 为平衡步兵
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++) {
@ -36,6 +36,8 @@ int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_c
if (distance_temp < distance_min) { if (distance_temp < distance_min) {
distance_min = distance_temp; distance_min = distance_temp;
idx = 1; idx = 1;
}else{
idx = 0;
} }
} else if (aim_sel->target_state.armor_num == 3)//前哨站 } else if (aim_sel->target_state.armor_num == 3)//前哨站
@ -43,62 +45,34 @@ int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_c
for (i = 0; i < 3; i++) { for (i = 0; i < 3; i++) {
float tmp_yaw; float tmp_yaw;
if (aim_sel->target_state.v_yaw <= 0) if (aim_sel->target_state.v_yaw <= 0)
tmp_yaw = aim_sel->target_state.yaw + i * (2.0 * PI / 3.0); tmp_yaw = aim_sel->target_state.yaw + i * (2.0f * PI / 3.0f);
else else
tmp_yaw = aim_sel->target_state.yaw - i * (2.0 * PI / 3.0); tmp_yaw = aim_sel->target_state.yaw - i * (2.0f * PI / 3.0f);
float r = use_1 ? aim_sel->target_state.r1 : aim_sel->target_state.r2; float r = use_1 ? aim_sel->target_state.r1 : aim_sel->target_state.r2;
aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * cos(tmp_yaw); aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * arm_cos_f32(tmp_yaw);
aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * sin(tmp_yaw); aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * arm_sin_f32(tmp_yaw);
aim_sel->armor_pose[i].z = use_1 ? aim_sel->target_state.z : aim_sel->target_state.z + aim_sel->armor_pose[i].z = use_1 ? aim_sel->target_state.z : aim_sel->target_state.z +
aim_sel->target_state.dz; aim_sel->target_state.dz;
aim_sel->armor_pose[i].yaw = aim_sel->target_state.yaw + i * (2.0 * PI / 3.0); aim_sel->armor_pose[i].yaw = aim_sel->target_state.yaw + i * (2.0f * PI / 3.0f);
use_1 = !use_1; use_1 = !use_1;
} }
// //计算枪管到目标装甲板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]; float distance_there[3];
for (i = 0; i < 3; i++) { for (i = 0; i < 3; i++) {
distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2); distance_there[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
} }
int label_first = 0; int label_first = 0;
int label_second = 0; int label_second = 0;
for (i = 1; i < 3; i++) { for (i = 1; i < 3; i++) {
if (distance[i] < distance[label_first]) { if (distance_there[i] < distance_there[label_first]) {
label_second = label_first; label_second = label_first;
label_first = i; label_first = i;
} else if (distance[i] < distance[label_second]) { } else if (distance_there[i] < distance_there[label_second] && distance_there[i] < distance_there[label_first]) {
label_second = i; label_second = i;
} else if (distance[i] < distance[label_second] && distance[i] < distance[label_first]) { } else if (distance_there[i] < distance_there[label_second]) {
label_second = i; label_second = i;
} }
} }
@ -122,7 +96,7 @@ int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_c
} 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.0f;
float r = use_1 ? aim_sel->target_state.r1 : aim_sel->target_state.r2; float r = use_1 ? aim_sel->target_state.r1 : aim_sel->target_state.r2;
aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * arm_cos_f32(tmp_yaw); aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * arm_cos_f32(tmp_yaw);
@ -144,21 +118,21 @@ int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_c
// } // }
// 选择两块较近的装甲板 // 选择两块较近的装甲板
float distance[4]; float distance_four[4];
for (i = 0; i < 4; i++) { for (i = 0; i < 4; i++) {
distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2); distance_four[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
} }
int label_first = 0; int label_first = 0;
int label_second = 0; int label_second = 0;
for (i = 1; i < 4; i++) { for (i = 1; i < 4; i++) {
if (distance[i] < distance[label_first]) { if (distance_four[i] < distance_four[label_first]) {
label_second = label_first; label_second = label_first;
label_first = i; label_first = i;
} else if (distance[i] < distance[label_second]) { } else if (distance_four[i] < distance_four[label_second] && distance_four[i] < distance_four[label_first]) {
label_second = i; label_second = i;
} else if (distance[i] < distance[label_second] && distance[i] < distance[label_first]) { }else if (distance_four[i] < distance_four[label_second]) {
label_second = i; label_second = i;
} }
} }
@ -235,15 +209,15 @@ int aim_center_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_
for (i = 0; i < 3; i++) { for (i = 0; i < 3; i++) {
float tmp_yaw; float tmp_yaw;
if (aim_sel->target_state.v_yaw <= 0) if (aim_sel->target_state.v_yaw <= 0)
tmp_yaw = aim_sel->target_state.yaw + i * (2.0 * PI / 3.0); tmp_yaw = aim_sel->target_state.yaw + i * (2.0f * PI / 3.0f);
else else
tmp_yaw = aim_sel->target_state.yaw - i * (2.0 * PI / 3.0); tmp_yaw = aim_sel->target_state.yaw - i * (2.0f * PI / 3.0f);
float r = use_1 ? aim_sel->target_state.r1 : aim_sel->target_state.r2; float r = use_1 ? aim_sel->target_state.r1 : aim_sel->target_state.r2;
aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * cos(tmp_yaw); aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * arm_cos_f32(tmp_yaw);
aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * sin(tmp_yaw); aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * arm_sin_f32(tmp_yaw);
aim_sel->armor_pose[i].z = use_1 ? aim_sel->target_state.z : aim_sel->target_state.z + aim_sel->armor_pose[i].z = use_1 ? aim_sel->target_state.z : aim_sel->target_state.z +
aim_sel->target_state.dz; aim_sel->target_state.dz;
aim_sel->armor_pose[i].yaw = aim_sel->target_state.yaw + i * (2.0 * PI / 3.0); aim_sel->armor_pose[i].yaw = aim_sel->target_state.yaw + i * (2.0f * PI / 3.0f);
use_1 = !use_1; use_1 = !use_1;
} }
@ -313,7 +287,7 @@ int aim_center_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_
} 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.0f;
float r = use_1 ? aim_sel->target_state.r1 : aim_sel->target_state.r2; float r = use_1 ? aim_sel->target_state.r1 : aim_sel->target_state.r2;
aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * arm_cos_f32(tmp_yaw); aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * arm_cos_f32(tmp_yaw);
@ -334,7 +308,7 @@ int aim_center_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_
// } // }
// } // }
// 选择两块较近的装甲板 // 选择两块较近de装甲板
float distance[4]; float distance[4];
for (i = 0; i < 4; i++) { for (i = 0; i < 4; i++) {
distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2); distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
@ -387,12 +361,12 @@ int aim_center_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_
* @param[in] v_x0:弹速水平分量 * @param[in] v_x0:弹速水平分量
* @retval 返回空 * @retval 返回空
*/ */
const float k1 = 0.015; //标准大气压25度下空气阻力系数 const float k1 = 0.015f; //标准大气压25度下空气阻力系数
float get_fly_time(float x, float vx, float v_x0) { float get_fly_time(float x, float vx, float v_x0) {
float t = 0; float t = 0;
float f_ti = 0, df_ti = 0; float f_ti = 0, df_ti = 0;
for (int i = 0; i < 5; i++) { for (int i = 0; i < 5; i++) {
f_ti = log(k1 * v_x0 * t + 1) / k1 - x - vx * t; f_ti = logf(k1 * v_x0 * t + 1) / k1 - x - vx * t;
df_ti = v_x0 / (k1 * v_x0 * t + 1) - vx; df_ti = v_x0 / (k1 * v_x0 * t + 1) - vx;
t = t - f_ti / df_ti; t = t - f_ti / df_ti;
} }
@ -461,7 +435,7 @@ int auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, Reci
aim_sel->target_state.r2 = receive_packet->r2; aim_sel->target_state.r2 = receive_packet->r2;
aim_sel->target_state.dz = receive_packet->dz; aim_sel->target_state.dz = receive_packet->dz;
int idx = 0; int idx = -1;
if(aim_sel->target_state.v_yaw >= 0.7* PI) if(aim_sel->target_state.v_yaw >= 0.7* PI)
{ {
mode_flag=0; mode_flag=0;

2
modules/auto_aim/auto_aim.h
View File

@ -65,7 +65,7 @@ typedef struct
Armor_Pose_Type_t armor_pose[4]; //四个装甲板状态 Armor_Pose_Type_t armor_pose[4]; //四个装甲板状态
float aim_point[3]; //预瞄点 float aim_point[3]; //预瞄点
float delay_time; //预瞄时间差 float delay_time; //预瞄时间差
uint8_t suggest_fire; int suggest_fire;
}Aim_Select_Type_t; }Aim_Select_Type_t;