From dba875719f54fafa0ba51a25cd4a55b419d88e43 Mon Sep 17 00:00:00 2001 From: shmily744 <1527550984@qq.com> Date: Mon, 22 Jan 2024 16:57:41 +0800 Subject: [PATCH] =?UTF-8?q?=E8=87=AA=E7=9E=84=E6=A8=A1=E5=9D=97=EF=BC=8C?= =?UTF-8?q?=E8=87=AA=E7=9E=84=E3=80=82=E8=AF=BB=E5=8F=96=E8=A3=81=E5=88=A4?= =?UTF-8?q?=E7=B3=BB=E7=BB=9F=E8=87=AA=E5=8A=A8=E8=AE=BE=E7=BD=AE=E6=95=8C?= =?UTF-8?q?=E6=96=B9=E9=A2=9C=E8=89=B2=EF=BC=88=E5=BE=85=E6=B5=8B=E8=AF=95?= =?UTF-8?q?=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 2 +- application/chassis/chassis.c | 9 +- application/cmd/robot_cmd.c | 56 ++++++- application/robot_def.h | 3 +- modules/auto_aim/auto_aim.c | 188 ++++++++++++++++++++++++ modules/master_machine/master_process.c | 4 +- modules/master_machine/master_process.h | 6 +- modules/referee/referee_protocol.h | 4 +- modules/referee/referee_task.c | 2 +- 9 files changed, 252 insertions(+), 22 deletions(-) create mode 100644 modules/auto_aim/auto_aim.c diff --git a/CMakeLists.txt b/CMakeLists.txt index b5c42ce..156504b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,7 +53,7 @@ include_directories(Inc Drivers/STM32F4xx_HAL_Driver/Inc Drivers/STM32F4xx_HAL_D bsp bsp/adc bsp/can bsp/dwt bsp/flash bsp/gpio bsp/iic bsp/log bsp/pwm bsp/spi bsp/usart bsp/usb modules modules/alarm modules/algorithm modules/BMI088 modules/can_comm modules/daemon modules/encoder modules/imu modules/ist8310 modules/led modules/master_machine modules/message_center modules/motor modules/oled modules/referee modules/remote modules/standard_cmd - modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa + modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa modules/auto_aim modules/motor/DJImotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor application application/chassis application/cmd application/gimbal application/shoot Middlewares/Third_Party/SEGGER/RTT Middlewares/Third_Party/SEGGER/Config) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 4412108..6fe7936 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -255,10 +255,11 @@ void ChassisTask() { // 根据电机的反馈速度和IMU(如果有)计算真实速度 EstimateSpeed(); - // // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送 - // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red - // chassis_feedback_data.enemy_color = referee_data->GameRobotState.robot_id > 7 ? 1 : 0; - // // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况 + //todo: 裁判系统信息移植到消息中心发送 + // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送 + // 发送敌方方颜色id + chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color; + // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况 // chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit; // chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0; diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index cc54ecf..13e23ff 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -8,6 +8,7 @@ #include "message_center.h" #include "general_def.h" #include "dji_motor.h" +#include "auto_aim.h" // bsp #include "bsp_dwt.h" #include "bsp_log.h" @@ -35,6 +36,12 @@ static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回 static RecievePacket_t *vision_recv_data; // 视觉接收数据指针,初始化时返回 static SendPacket_t vision_send_data; // 视觉发送数据 +//自瞄相关信息 +static Trajectory_Type_t trajectory_cal; +static Aim_Select_Type_t aim_select; +static uint32_t no_find_cnt; // 未发现目标计数 +static uint8_t auto_aim_flag = 0; //辅助瞄准标志位 视野内有目标开启 目标丢失关闭 + static Publisher_t *gimbal_cmd_pub; // 云台控制消息发布者 static Subscriber_t *gimbal_feed_sub; // 云台反馈信息订阅者 static Gimbal_Ctrl_Cmd_s gimbal_cmd_send; // 传递给云台的控制信息 @@ -120,19 +127,53 @@ static void RemoteControlSet() { } // 云台参数,确定云台控制数据 - if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[中],视觉模式 + if (switch_is_mid(rc_data[TEMP].rc.switch_left) || + (vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0 + && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 && + vision_recv_data->vz == 0)) // 左侧开关状态为[中],或视觉未识别到目标,纯遥控器拨杆控制 { // 待添加,视觉会发来和目标的误差,同样将其转化为total angle的增量进行控制 // ... - } - // 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制 - if (switch_is_down(rc_data[TEMP].rc.switch_left))//|| vision_recv_data->target_state == NO_TARGET - { // 按照摇杆的输出大小进行角度增量,增益系数需调整 gimbal_cmd_send.yaw -= 0.0025f * (float) rc_data[TEMP].rc.rocker_l_; gimbal_cmd_send.pitch -= 0.001f * (float) rc_data[TEMP].rc.rocker_l1; if (gimbal_cmd_send.pitch >= PITCH_MAX_ANGLE) gimbal_cmd_send.pitch = PITCH_MAX_ANGLE; if (gimbal_cmd_send.pitch <= PITCH_MIN_ANGLE) gimbal_cmd_send.pitch = PITCH_MIN_ANGLE; + + + } + // 左侧开关状态为[下],视觉模式 + if (switch_is_down(rc_data[TEMP].rc.switch_left)) { + trajectory_cal.v0 = 30; //弹速30 + if (vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0 + && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 && vision_recv_data->vz == 0) { + aim_select.suggest_fire = 0; + //未发现目标 + no_find_cnt++; + + if (no_find_cnt >= 2000) { + //gimbal_scan_flag = 1; + //auto_aim_flag = 0; + } + //else + //auto_aim_flag = 1; + } else { + //弹道解算 + no_find_cnt = 0; + auto_aim_flag = 1; + + 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); + if (yaw_err <= 0.008) //3度 + aim_select.suggest_fire = 1; + else + aim_select.suggest_fire = 0; + } } // 云台软件限位 @@ -284,7 +325,8 @@ void RobotCMDTask() { // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成 CalcOffsetAngle(); // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠 - if (switch_is_down(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制 + if (switch_is_down(rc_data[TEMP].rc.switch_left) || + switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],[中],遥控器控制 RemoteControlSet(); else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制 MouseKeySet(); @@ -292,7 +334,7 @@ void RobotCMDTask() { EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况 // 设置视觉发送数据,还需增加加速度和角速度数据 - //VisionSetFlag(chassis_fetch_data.enemy_color,chassis_fetch_data.bullet_speed); + VisionSetFlag(chassis_fetch_data.enemy_color); // 推送消息,双板通信,视觉通信等 // 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置 diff --git a/application/robot_def.h b/application/robot_def.h index 58f2280..1f88f6a 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -189,8 +189,7 @@ typedef struct uint8_t rest_heat; // 剩余枪口热量 Bullet_Speed_e bullet_speed; // 弹速限制 - Enemy_Color_e enemy_color; // 0 for blue, 1 for red - + Enemy_Color_e enemy_color; // 0 for red, 1 for blue } Chassis_Upload_Data_s; diff --git a/modules/auto_aim/auto_aim.c b/modules/auto_aim/auto_aim.c new file mode 100644 index 0000000..c0f4e36 --- /dev/null +++ b/modules/auto_aim/auto_aim.c @@ -0,0 +1,188 @@ +// +// Created by sph on 2024/1/21. +// +#include "auto_aim.h" +#include "arm_math.h" + +/** + * @brief 选择目标装甲板 + * @author SJQ + * @param[in] trajectory_cal:弹道解算结构体 + * @retval 返回空 + */ +void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal) { + aim_sel->delay_time = trajectory_cal->fly_time + trajectory_cal->extra_delay_time; + aim_sel->target_state.yaw += aim_sel->target_state.v_yaw * aim_sel->delay_time; + + //计算四块装甲板的位置 + int use_1 = 1; + int i = 0; + int idx = 0; // 选择的装甲板 + // 为平衡步兵 + if (aim_sel->target_state.armor_num == 2) { + for (i = 0; i < 2; i++) { + float tmp_yaw = aim_sel->target_state.yaw + i * PI; + float r = aim_sel->target_state.r1; + + 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 * arm_sin_f32(tmp_yaw); + aim_sel->armor_pose[i].z = aim_sel->target_state.z; + 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; + idx = 1; + } + } else if (aim_sel->target_state.armor_num == 3)//前哨站 + { + for (i = 0; i < 3; i++) { + float tmp_yaw; + if (aim_sel->target_state.v_yaw <= 0) + tmp_yaw = aim_sel->target_state.yaw + i * (2.0 * PI / 3.0); + else + tmp_yaw = aim_sel->target_state.yaw - i * (2.0 * PI / 3.0); + 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].y = aim_sel->target_state.y - r * sin(tmp_yaw); + 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->armor_pose[i].yaw = aim_sel->target_state.yaw + i * (2.0 * PI / 3.0); + 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; + } + } + } else { + for (i = 0; i < 4; i++) { + float tmp_yaw = aim_sel->target_state.yaw + i * PI / 2.0; + 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].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->target_state.dz; + aim_sel->armor_pose[i].yaw = tmp_yaw; + use_1 = !use_1; + } + + //计算枪管到目标装甲板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; + } + } + } + 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; +} + +/** + * @brief 子弹飞行时间解算 + * @author SJQ + * @param[in] x:瞄准时shooter_link下目标x坐标 + * @param[in] vx:瞄准时shooter_link下目标x速度 + * @param[in] v_x0:弹速水平分量 + * @retval 返回空 + */ +const float k1 = 0.0761; //标准大气压25度下空气阻力系数 +float get_fly_time(float x, float vx, float v_x0) { + float t = 0; + float f_ti = 0, df_ti = 0; + for (int i = 0; i < 5; i++) { + f_ti = log(k1 * v_x0 * t + 1) / k1 - x - vx * t; + df_ti = v_x0 / (k1 * v_x0 * t + 1) - vx; + t = t - f_ti / df_ti; + } + return t; +} + +/** + * @brief 解算期望云台角度（考虑子弹飞行时间） + * @author SJQ + * @param[in] trajectory_cal:弹道解算结构体 + * @retval 返回空 + */ +void get_cmd_angle(Trajectory_Type_t *trajectory_cal) { + + arm_power_f32(trajectory_cal->position_xy, 2, &trajectory_cal->dis2); + arm_sqrt_f32(trajectory_cal->dis2, &trajectory_cal->dis); + + trajectory_cal->dis = trajectory_cal->dis - 0.20; + + trajectory_cal->theta_0 = atan2f(trajectory_cal->z, trajectory_cal->dis); + trajectory_cal->alpha = atan2f(trajectory_cal->position_xy[1], trajectory_cal->position_xy[0]); + //将目标的xyz速度转化为平行枪管与垂直枪管的速度 + trajectory_cal->vx = trajectory_cal->velocity[0] * arm_cos_f32(trajectory_cal->alpha) + + trajectory_cal->velocity[1] * arm_sin_f32(trajectory_cal->alpha); + trajectory_cal->vy = -trajectory_cal->velocity[0] * arm_sin_f32(trajectory_cal->alpha) + + trajectory_cal->velocity[1] * arm_cos_f32(trajectory_cal->alpha); + + float v_x0 = trajectory_cal->v0 * arm_cos_f32(trajectory_cal->theta_0);//水平方向弹速 + float v_y0 = trajectory_cal->v0 * arm_sin_f32(trajectory_cal->theta_0);//竖直方向弹速 + + trajectory_cal->fly_time = get_fly_time(trajectory_cal->dis, trajectory_cal->vx, v_x0); + arm_power_f32(&trajectory_cal->fly_time, 1, &trajectory_cal->fly_time2); + trajectory_cal->h_r = trajectory_cal->z + trajectory_cal->velocity[2] * trajectory_cal->fly_time; + //开始迭代 + trajectory_cal->theta_k = trajectory_cal->theta_0; + trajectory_cal->k = 0; + while (trajectory_cal->k < 10) { + v_y0 = trajectory_cal->v0 * arm_sin_f32(trajectory_cal->theta_k);//竖直方向弹速 + trajectory_cal->h_k = v_y0 * trajectory_cal->fly_time - 0.5 * 9.8 * trajectory_cal->fly_time2; + trajectory_cal->err_k = trajectory_cal->h_r - trajectory_cal->h_k; + trajectory_cal->theta_k += 0.1 * trajectory_cal->err_k; + trajectory_cal->k++; + if (trajectory_cal->err_k < 0.005) break; + } + + trajectory_cal->cmd_yaw = atan2f(trajectory_cal->position_xy[1], + trajectory_cal->position_xy[0]); + + trajectory_cal->cmd_pitch = trajectory_cal->theta_k; +} + +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 + + aim_sel->target_state.armor_type = receive_packet->id; + aim_sel->target_state.armor_num = receive_packet->armors_num; + aim_sel->target_state.x = receive_packet->x; + aim_sel->target_state.y = receive_packet->y; + aim_sel->target_state.z = receive_packet->z; + aim_sel->target_state.vx = receive_packet->vx; + aim_sel->target_state.vy = receive_packet->vy; + aim_sel->target_state.vz = receive_packet->vz; + aim_sel->target_state.yaw = receive_packet->yaw; + aim_sel->target_state.v_yaw = receive_packet->v_yaw; + aim_sel->target_state.r1 = receive_packet->r1; + aim_sel->target_state.r2 = receive_packet->r2; + aim_sel->target_state.dz = receive_packet->dz; + + aim_armor_select(aim_sel, trajectory_cal); + + trajectory_cal->position_xy[0] = aim_sel->aim_point[0]; + trajectory_cal->position_xy[1] = aim_sel->aim_point[1]; + trajectory_cal->z = aim_sel->aim_point[2]; + trajectory_cal->velocity[0] = aim_sel->target_state.vx; + trajectory_cal->velocity[1] = aim_sel->target_state.vy; + trajectory_cal->velocity[2] = aim_sel->target_state.vz; + + get_cmd_angle(trajectory_cal); + +} diff --git a/modules/master_machine/master_process.c b/modules/master_machine/master_process.c index 4942c1b..1931839 100644 --- a/modules/master_machine/master_process.c +++ b/modules/master_machine/master_process.c @@ -27,7 +27,7 @@ static DaemonInstance *vision_daemon_instance; //} void VisionSetFlag(Enemy_Color_e enemy_color) { - send_data.robot_color = enemy_color; + send_data.detect_color = enemy_color; send_data.reserved = 0; } @@ -177,7 +177,7 @@ void VisionSend() static uint8_t send_buffer[24]={0}; send_data.header = 0x5A; - VisionSetFlag(1); + //VisionSetFlag(1); VisionSetAim(recv_data.x,recv_data.y,recv_data.z); send_data.checksum = crc_16(&send_data.header,sizeof(send_data)-2); diff --git a/modules/master_machine/master_process.h b/modules/master_machine/master_process.h index 9101597..405912a 100644 --- a/modules/master_machine/master_process.h +++ b/modules/master_machine/master_process.h @@ -45,8 +45,8 @@ typedef enum { // COLOR_NONE = 0, // COLOR_BLUE = 1, // COLOR_RED = 2, - COLOR_RED = 0, - COLOR_BLUE = 1, + ENEMY_COLOR_RED = 0, + ENEMY_COLOR_BLUE = 1, } Enemy_Color_e; typedef enum { @@ -76,7 +76,7 @@ typedef struct { typedef __packed struct { uint8_t header;//0x5A - uint8_t robot_color: 1; + uint8_t detect_color: 1; uint8_t reserved: 7; float pitch; float yaw; diff --git a/modules/referee/referee_protocol.h b/modules/referee/referee_protocol.h index 22416b3..0070412 100644 --- a/modules/referee/referee_protocol.h +++ b/modules/referee/referee_protocol.h @@ -16,8 +16,8 @@ /****************************宏定义部分****************************/ #define REFEREE_SOF 0xA5 // 起始字节,协议固定为0xA5 -#define Robot_Red 0 -#define Robot_Blue 1 +#define Robot_Red 1 +#define Robot_Blue 0 #define Communicate_Data_LEN 5 // 自定义交互数据长度，该长度决定了我方发送和他方接收，自定义交互数据协议更改时只需要更改此宏定义即可 #pragma pack(1) diff --git a/modules/referee/referee_task.c b/modules/referee/referee_task.c index 7971265..9950066 100644 --- a/modules/referee/referee_task.c +++ b/modules/referee/referee_task.c @@ -28,7 +28,7 @@ uint8_t UI_Seq; // 包序号，供整个ref */ static void DeterminRobotID() { - // id小于7是红色,大于7是蓝色,0为红色，1为蓝色 #define Robot_Red 0 #define Robot_Blue 1 + // id小于7是红色,大于7是蓝色 #define Robot_Red 0 #define Robot_Blue 1 referee_recv_info->referee_id.Robot_Color = referee_recv_info->GameRobotState.robot_id > 7 ? Robot_Blue : Robot_Red; referee_recv_info->referee_id.Robot_ID = referee_recv_info->GameRobotState.robot_id; referee_recv_info->referee_id.Cilent_ID = 0x0100 + referee_recv_info->referee_id.Robot_ID; // 计算客户端ID