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

底盘跟随，5V激光，拨弹轮

This commit is contained in:
周楚杰 2024-01-27 10:41:36 +08:00
parent 7fc13a67e6
commit 26e53fa916
12 changed files with 481 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
CMakeLists.txt
View File

@ -4,7 +4,6 @@ set(CMAKE_SYSTEM_VERSION 1)
cmake_minimum_required(VERSION 3.25) cmake_minimum_required(VERSION 3.25)
# specify cross-compilers and tools # specify cross-compilers and tools
set(CMAKE_C_COMPILER arm-none-eabi-gcc)
set(CMAKE_CXX_COMPILER arm-none-eabi-g++) set(CMAKE_CXX_COMPILER arm-none-eabi-g++)
set(CMAKE_ASM_COMPILER arm-none-eabi-gcc) set(CMAKE_ASM_COMPILER arm-none-eabi-gcc)
set(CMAKE_AR arm-none-eabi-ar) set(CMAKE_AR arm-none-eabi-ar)
@ -53,7 +52,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 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 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/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 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 application application/chassis application/cmd application/gimbal application/shoot
Middlewares/Third_Party/SEGGER/RTT Middlewares/Third_Party/SEGGER/Config) Middlewares/Third_Party/SEGGER/RTT Middlewares/Third_Party/SEGGER/Config)
@ -68,7 +67,8 @@ add_definitions(-DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx)
file(GLOB_RECURSE SOURCES "Startup/*.*" "Src/*.*" "Middlewares/*.*" "Drivers/*.*" file(GLOB_RECURSE SOURCES "Startup/*.*" "Src/*.*" "Middlewares/*.*" "Drivers/*.*"
"bsp/*.*" "application/*.*" "modules/*.*") "bsp/*.*" "application/*.*" "modules/*.*")
set(LINKER_SCRIPT ${CMAKE_SOURCE_DIR}/STM32F407IGHX_FLASH.ld) set(LINKER_SCRIPT ${CMAKE_SOURCE_DIR}/STM32F407IGHX_FLASH.ld
)
add_link_options(-Wl,-gc-sections,--print-memory-usage,-Map=${PROJECT_BINARY_DIR}/${PROJECT_NAME}.map) add_link_options(-Wl,-gc-sections,--print-memory-usage,-Map=${PROJECT_BINARY_DIR}/${PROJECT_NAME}.map)
add_link_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork) add_link_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork)

2
application/chassis/chassis.c
View File

@ -242,7 +242,7 @@ void ChassisTask()
// // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送 // // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
// // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
// chassis_feedback_data.enemy_color = referee_data->GameRobotState.robot_id > 7 ? 1 : 0; chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color;
// // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况 // // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
// chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit; // chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit;
// chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0; // chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;

66
application/cmd/robot_cmd.c
View File

@ -8,6 +8,7 @@
#include "message_center.h" #include "message_center.h"
#include "general_def.h" #include "general_def.h"
#include "dji_motor.h" #include "dji_motor.h"
#include "auto_aim.h"
// bsp // bsp
#include "bsp_dwt.h" #include "bsp_dwt.h"
#include "bsp_log.h" #include "bsp_log.h"
@ -30,8 +31,16 @@ static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信
static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等 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; // 视觉接收数据指针,初始化时返回
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 Publisher_t *gimbal_cmd_pub; // 云台控制消息发布者
static Subscriber_t *gimbal_feed_sub; // 云台反馈信息订阅者 static Subscriber_t *gimbal_feed_sub; // 云台反馈信息订阅者
@ -122,27 +131,58 @@ static void RemoteControlSet()
chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
} }
else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动 else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘跟随云台
{ {
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE; gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
} }
// 云台参数,确定云台控制数据 // 云台参数,确定云台控制数据
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.003f * (float)rc_data[TEMP].rc.rocker_l_; gimbal_cmd_send.yaw += 0.003f * (float)rc_data[TEMP].rc.rocker_l_;
gimbal_cmd_send.pitch += 0.0007f * (float)rc_data[TEMP].rc.rocker_l1; gimbal_cmd_send.pitch += 0.0007f * (float)rc_data[TEMP].rc.rocker_l1;
if(gimbal_cmd_send.pitch>=18.0) gimbal_cmd_send.pitch=18.0f; if(gimbal_cmd_send.pitch>=18.0) gimbal_cmd_send.pitch=18.0f;
if(gimbal_cmd_send.pitch<=-40.0) gimbal_cmd_send.pitch=-40.0f; if(gimbal_cmd_send.pitch<=-40.0) gimbal_cmd_send.pitch=-40.0f;
} }
// 左侧开关状态为[下],视觉模式
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;
}
}
// 云台软件限位 // 云台软件限位
// 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整 // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
@ -298,7 +338,8 @@ void RobotCMDTask()
// 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成 // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成
CalcOffsetAngle(); 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(); RemoteControlSet();
else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制 else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制
MouseKeySet(); MouseKeySet();
@ -306,7 +347,8 @@ void RobotCMDTask()
EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况 EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
// 设置视觉发送数据,还需增加加速度和角速度数据 // 设置视觉发送数据,还需增加加速度和角速度数据
// VisionSetFlag(chassis_fetch_data.enemy_color,,chassis_fetch_data.bullet_speed) VisionSetFlag(chassis_fetch_data.enemy_color);
// 推送消息,双板通信,视觉通信等 // 推送消息,双板通信,视觉通信等
// 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置 // 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置

67
modules/algorithm/crc16.c
View File

@ -1,7 +1,29 @@
#include "crc16.h" #include "crc16.h"
static uint8_t crc_tab16_init = 0; static uint8_t crc_tab16_init = 0;
static uint16_t crc_tab16[256]; static uint16_t crc_tab16[256] = {
0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48, 0x9dc1, 0xaf5a, 0xbed3,
0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e,
0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399,
0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5,
0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50,
0xfbef, 0xea66, 0xd8fd, 0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285, 0x430c, 0x7197, 0x601e,
0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72,
0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5,
0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738,
0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693,
0xc22c, 0xd3a5, 0xe13e, 0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1, 0x0948, 0x3bd3, 0x2a5a,
0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5,
0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710,
0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c,
0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df,
0x0c60, 0x1de9, 0x2f72, 0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e, 0xf687, 0xc41c, 0xd595,
0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9,
0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c,
0x3de3, 0x2c6a, 0x1ef1, 0x0f78};
/* /*
* uint16_t crc_16( const unsigned char *input_str, size_t num_bytes ); * uint16_t crc_16( const unsigned char *input_str, size_t num_bytes );
@ -13,19 +35,28 @@ static uint16_t crc_tab16[256];
*/ */
uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes) uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes)
{ {
uint16_t crc; // uint16_t crc;
const uint8_t *ptr; // const uint8_t *ptr;
uint16_t a; // uint16_t a;
if (!crc_tab16_init) //// if (!crc_tab16_init)
init_crc16_tab(); //// init_crc16_tab();
crc = CRC_START_16; // crc = CRC_START_16;
ptr = input_str; // ptr = input_str;
if (ptr != NULL) // if (ptr != NULL)
for (a = 0; a < num_bytes; a++) // for (a = 0; a < num_bytes; a++) {
{ // crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t) *ptr++) & 0x00FF];
crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t)*ptr++) & 0x00FF]; // }
// return crc;
uint8_t ch_data;
uint16_t wCRC = 0xFFFF;
if (input_str == NULL) return 0xFFFF;
while (num_bytes--) {
ch_data = *input_str++;
(wCRC) =
((uint16_t)(wCRC) >> 8) ^ crc_tab16[((uint16_t)(wCRC) ^ (uint16_t)(ch_data)) & 0x00ff];
} }
return crc; return wCRC;
} }
/* /*
@ -99,3 +130,13 @@ void init_crc16_tab(void)
} }
crc_tab16_init = 1; crc_tab16_init = 1;
} }
uint32_t VerifyCRC16CheckSum(uint8_t *pchMessage, uint32_t dwLength)
{
uint16_t wExpected = 0;
if ((pchMessage == NULL) || (dwLength <= 2))
{
return 0;
}
wExpected = crc_16(pchMessage, dwLength - 2);
return ((wExpected & 0xff) == pchMessage[dwLength - 2] && ((wExpected >> 8) & 0xff) == pchMessage[dwLength - 1]);
}

1
modules/algorithm/crc16.h
View File

@ -10,5 +10,6 @@ uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes);
uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes); uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes);
uint16_t update_crc_16(uint16_t crc, uint8_t c); uint16_t update_crc_16(uint16_t crc, uint8_t c);
void init_crc16_tab(void); void init_crc16_tab(void);
uint32_t VerifyCRC16CheckSum(uint8_t *pchMessage, uint32_t dwLength);
#endif #endif

188
modules/auto_aim/auto_aim.c Normal file
View File

@ -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);
}

77
modules/auto_aim/auto_aim.h Normal file
View File

@ -0,0 +1,77 @@
//
// Created by sph on 2024/1/21.
//
#ifndef BASIC_FRAMEWORK_AUTO_AIM_H
#define BASIC_FRAMEWORK_AUTO_AIM_H
#include "master_process.h"
//弹道解算
typedef struct
{
float v0; //子弹射速
float velocity[3];//目标xyz速度
float vx; //目标相对枪口方向的速度
float vy;
float alpha; //目标初始航向角
float position_xy[2];//目标xy坐标
float z; //目标z坐标
float fly_time; //子弹飞行时间
float fly_time2; //子弹飞行时间平方
float extra_delay_time ;
float theta_0; //初始目标角度
float theta_k; //迭代目标角度
float dis; //目标距离
float dis2; //目标距离平方
float err_k; //迭代误差
uint8_t k; //迭代次数
float h_k; //迭代高度
float h_r; //目标真实高度
float cmd_yaw;
float cmd_pitch;
} Trajectory_Type_t;
//整车状态
typedef struct
{
float x;
float y;
float z;
float yaw;
float vx;
float vy;
float vz;
float v_yaw;
float r1;
float r2;
float dz;
uint8_t armor_type;
uint8_t armor_num;
}Target_State_Type_t;
//预瞄点
typedef struct
{
float x;
float y;
float z;
float yaw;
}Armor_Pose_Type_t;
typedef struct
{
Target_State_Type_t target_state; //整车状态
Armor_Pose_Type_t armor_pose[4]; //四个装甲板状态
float aim_point[3]; //预瞄点
float delay_time; //预瞄时间差
uint8_t suggest_fire;
}Aim_Select_Type_t;
void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal);
float get_fly_time(float x, float vx, float v_x0);
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);
#endif //BASIC_FRAMEWORK_AUTO_AIM_H

8
modules/imu/ins_task.c
View File

@ -19,6 +19,7 @@
#include "user_lib.h" #include "user_lib.h"
#include "general_def.h" #include "general_def.h"
#include "master_process.h" #include "master_process.h"
#include "crc16.h"
static INS_t INS; static INS_t INS;
static IMU_Param_t IMU_Param; static IMU_Param_t IMU_Param;
@ -158,8 +159,8 @@ void INS_Task(void)
EarthFrameToBodyFrame(gravity, gravity_b, INS.q); EarthFrameToBodyFrame(gravity, gravity_b, INS.q);
for (uint8_t i = 0; i < 3; ++i) // 同样过一个低通滤波 for (uint8_t i = 0; i < 3; ++i) // 同样过一个低通滤波
{ {
INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt); INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) +
} INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt); }
BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n
INS.Yaw = QEKF_INS.Yaw; INS.Yaw = QEKF_INS.Yaw;
@ -167,7 +168,8 @@ void INS_Task(void)
INS.Roll = QEKF_INS.Roll; INS.Roll = QEKF_INS.Roll;
INS.YawTotalAngle = QEKF_INS.YawTotalAngle; INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll); //VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll);
VisionSetAltitude(INS.Yaw * PI / 180, INS.Pitch * PI / 180);
} }
// temperature control // temperature control

69
modules/master_machine/master_process.c
View File

@ -13,25 +13,36 @@
#include "daemon.h" #include "daemon.h"
#include "bsp_log.h" #include "bsp_log.h"
#include "robot_def.h" #include "robot_def.h"
#include "crc16.h"
static Vision_Recv_s recv_data; static RecievePacket_t recv_data;
static Vision_Send_s send_data; static SendPacket_t send_data;
static DaemonInstance *vision_daemon_instance; static DaemonInstance *vision_daemon_instance;
void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed) //void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed)
//{
// send_data.enemy_color = enemy_color;
// send_data.work_mode = work_mode;
// send_data.bullet_speed = bullet_speed;
//}
void VisionSetFlag(Enemy_Color_e enemy_color)
{ {
send_data.enemy_color = enemy_color; send_data.detect_color = enemy_color;
send_data.work_mode = work_mode; send_data.reserved = 0;
send_data.bullet_speed = bullet_speed;
} }
void VisionSetAltitude(float yaw, float pitch, float roll) void VisionSetAltitude(float yaw, float pitch)
{ {
send_data.yaw = yaw; send_data.yaw = yaw;
send_data.pitch = pitch; send_data.pitch = pitch;
send_data.roll = roll;
} }
void VisionSetAim(float aim_x, float aim_y, float aim_z)
{
send_data.aim_x = aim_x;
send_data.aim_y = aim_y;
send_data.aim_z = aim_z;
}
/** /**
* @brief 离线回调函数,将在daemon.c中被daemon task调用 * @brief 离线回调函数,将在daemon.c中被daemon task调用
* @attention 由于HAL库的设计问题,串口开启DMA接收之后同时发送有概率出现__HAL_LOCK()导致的死锁,使得无法 * @attention 由于HAL库的设计问题,串口开启DMA接收之后同时发送有概率出现__HAL_LOCK()导致的死锁,使得无法
@ -113,17 +124,24 @@ void VisionSend()
#ifdef VISION_USE_VCP #ifdef VISION_USE_VCP
#include "bsp_usb.h" #include "bsp_usb.h"
static uint8_t *vis_recv_buff; static uint8_t *vis_recv_buff; //接收到的原始数据
static void DecodeVision(uint16_t recv_len) static void DecodeVision(uint16_t recv_len)
{ {
uint16_t flag_register; // uint16_t flag_register;
get_protocol_info(vis_recv_buff, &flag_register, (uint8_t *)&recv_data.pitch); // get_protocol_info(vis_recv_buff, &flag_register, (uint8_t *)&recv_data.pitch);
// TODO: code to resolve flag_register; // // TODO: code to resolve flag_register;
if(vis_recv_buff[0]==0xA5)
{
if(VerifyCRC16CheckSum(vis_recv_buff,sizeof(recv_data)))
{
memcpy(&recv_data,vis_recv_buff,sizeof(recv_data));
}
}
} }
/* 视觉通信初始化 */ /* 视觉通信初始化 */
Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle) RecievePacket_t *VisionInit(UART_HandleTypeDef *_handle)
{ {
UNUSED(_handle); // 仅为了消除警告 UNUSED(_handle); // 仅为了消除警告
USB_Init_Config_s conf = {.rx_cbk = DecodeVision}; USB_Init_Config_s conf = {.rx_cbk = DecodeVision};
@ -142,14 +160,23 @@ Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle)
void VisionSend() void VisionSend()
{ {
static uint16_t flag_register; // static uint16_t flag_register;
static uint8_t send_buff[VISION_SEND_SIZE]; // static uint8_t send_buff[VISION_SEND_SIZE];
static uint16_t tx_len; // static uint16_t tx_len;
// TODO: code to set flag_register // // TODO: code to set flag_register
flag_register = 30 << 8 | 0b00000001; // flag_register = 30 << 8 | 0b00000001;
// 将数据转化为seasky协议的数据包 // // 将数据转化为seasky协议的数据包
get_protocol_send_data(0x02, flag_register, &send_data.yaw, 3, send_buff, &tx_len); // get_protocol_send_data(0x02, flag_register, &send_data.yaw, 3, send_buff, &tx_len);
USBTransmit(send_buff, tx_len); // USBTransmit(send_buff, tx_len);
static uint8_t send_buffer[24]={0};
send_data.header = 0x5A;
// VisionSetFlag(1);
VisionSetAim(recv_data.x,recv_data.y,recv_data.z);
send_data.checksum = crc_16(&send_data.header,sizeof(send_data)-2);
memcpy(send_buffer,&send_data,sizeof(send_data));
USBTransmit(send_buffer, sizeof(send_data));
} }
#endif // VISION_USE_VCP #endif // VISION_USE_VCP

53
modules/master_machine/master_process.h
View File

@ -47,9 +47,11 @@ typedef struct
typedef enum typedef enum
{ {
COLOR_NONE = 0, // COLOR_NONE = 0,
COLOR_BLUE = 1, // COLOR_BLUE = 1,
COLOR_RED = 2, // COLOR_RED = 2,
ENEMY_COLOR_RED = 0,
ENEMY_COLOR_BLUE = 1,
} Enemy_Color_e; } Enemy_Color_e;
typedef enum typedef enum
@ -79,6 +81,40 @@ typedef struct
float pitch; float pitch;
float roll; float roll;
} Vision_Send_s; } Vision_Send_s;
typedef __packed struct {
uint8_t header;//0x5A
uint8_t detect_color: 1;
uint8_t reserved: 7;
float pitch;
float yaw;
float aim_x;
float aim_y;
float aim_z;
uint16_t checksum;
} SendPacket_t;
typedef __packed struct
{
uint8_t header; //= 0xA5;
uint8_t tracking: 1;
uint8_t id: 3; // 0-outpost 6-guard 7-base
uint8_t armors_num: 3; // 2-balance 3-outpost 4-normal
uint8_t reserved: 1;
float x;
float y;
float z;
float yaw;
float vx;
float vy;
float vz;
float v_yaw;
float r1;
float r2;
float dz;
uint16_t checksum;
} RecievePacket_t;
#pragma pack() #pragma pack()
/** /**
@ -86,7 +122,8 @@ typedef struct
* *
* @param handle 用于和视觉通信的串口handle(C板上一般为USART1,丝印为USART2,4pin) * @param handle 用于和视觉通信的串口handle(C板上一般为USART1,丝印为USART2,4pin)
*/ */
Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle); //Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle);
RecievePacket_t *VisionInit(UART_HandleTypeDef *_handle);
/** /**
* @brief 发送视觉数据 * @brief 发送视觉数据
@ -101,14 +138,16 @@ void VisionSend();
* @param work_mode * @param work_mode
* @param bullet_speed * @param bullet_speed
*/ */
void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed); //void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed);
void VisionSetFlag(Enemy_Color_e enemy_color);
/** /**
* @brief 设置发送数据的姿态部分 * @brief 设置发送数据的姿态部分
* *
* @param yaw * @param yaw
* @param pitch * @param pitch
*/ */
void VisionSetAltitude(float yaw, float pitch, float roll); //void VisionSetAltitude(float yaw, float pitch, float roll);
void VisionSetAltitude(float yaw, float pitch);
void VisionSetAim(float aim_x, float aim_y,float aim_z);
#endif // !MASTER_PROCESS_H #endif // !MASTER_PROCESS_H

4
modules/referee/referee_protocol.h
View File

@ -16,8 +16,8 @@
/****************************宏定义部分****************************/ /****************************宏定义部分****************************/
#define REFEREE_SOF 0xA5 // 起始字节,协议固定为0xA5 #define REFEREE_SOF 0xA5 // 起始字节,协议固定为0xA5
#define Robot_Red 0 #define Robot_Red 1
#define Robot_Blue 1 #define Robot_Blue 0
#define Communicate_Data_LEN 5 // 自定义交互数据长度，该长度决定了我方发送和他方接收，自定义交互数据协议更改时只需要更改此宏定义即可 #define Communicate_Data_LEN 5 // 自定义交互数据长度，该长度决定了我方发送和他方接收，自定义交互数据协议更改时只需要更改此宏定义即可
#pragma pack(1) #pragma pack(1)

2
modules/referee/referee_task.c
View File

@ -28,7 +28,7 @@ uint8_t UI_Seq; // 包序号，供整个ref
*/ */
static void DeterminRobotID() 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_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.Robot_ID = referee_recv_info->GameRobotState.robot_id;
referee_recv_info->referee_id.Cilent_ID = 0x0100 + referee_recv_info->referee_id.Robot_ID; // 计算客户端ID referee_recv_info->referee_id.Cilent_ID = 0x0100 + referee_recv_info->referee_id.Robot_ID; // 计算客户端ID