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

自瞄选板修改

This commit is contained in:
shmily744 2024-05-14 02:34:46 +08:00
parent 5ef2702d27
commit 6ca069c064
4 changed files with 34 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
CMakeLists.txt
View File

@ -4,6 +4,7 @@ set(CMAKE_SYSTEM_VERSION 1)
cmake_minimum_required(VERSION 3.25)
# specify cross-compilers and tools
set(CMAKE_C_COMPILER arm-none-eabi-gcc)
set(CMAKE_CXX_COMPILER arm-none-eabi-g++)
set(CMAKE_ASM_COMPILER arm-none-eabi-gcc)
set(CMAKE_AR arm-none-eabi-ar)
@ -51,7 +52,7 @@ endif ()
include_directories(Inc Drivers/STM32F4xx_HAL_Driver/Inc Drivers/STM32F4xx_HAL_Driver/Inc/Legacy Middlewares/Third_Party/FreeRTOS/Source/include Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F Middlewares/ST/STM32_USB_Device_Library/Core/Inc Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc Drivers/CMSIS/Device/ST/STM32F4xx/Include Drivers/CMSIS/Include Middlewares/ST/ARM/DSP/Inc
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/led modules/master_machine modules/message_center modules/motor modules/oled modules/power_meter modules/referee modules/remote modules/standard_cmd
modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa modules/auto_aim
modules/motor/DJImotor modules/motor/DMmotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor
application application/chassis application/cmd application/gimbal application/shoot
@ -73,8 +74,7 @@ add_link_options(-Wl,-gc-sections,--print-memory-usage,-Map=${PROJECT_BINARY_DIR
add_link_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork)
add_link_options(-T ${LINKER_SCRIPT})
add_executable(${PROJECT_NAME}.elf ${SOURCES} ${LINKER_SCRIPT}
modules/power_meter/power_meter.c)
add_executable(${PROJECT_NAME}.elf ${SOURCES} ${LINKER_SCRIPT})
target_link_libraries(${PROJECT_NAME}.elf ${CMAKE_SOURCE_DIR}/Middlewares/ST/ARM/DSP/Lib/libarm_cortexM4lf_math.a)

29
application/cmd/robot_cmd.c
View File

@ -30,6 +30,7 @@ static Publisher_t *chassis_cmd_pub; // 底盘控制消息发布者
static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者
#endif // ONE_BOARD
static float temp_yaw; //for test
static float temp_yaw_err; //for test
static int temp_index; //for test
static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关
@ -165,7 +166,7 @@ static void pitch_limit()
}
static void auto_aim_mode()
{
trajectory_cal.v0 = 14.5f; //弹速30
trajectory_cal.v0 = 13.5f; //弹速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;
@ -199,20 +200,30 @@ static void auto_aim_mode()
gimbal_cmd_send.pitch = -trajectory_cal.cmd_pitch * 180 / PI + 0.2 * trajectory_cal.dis;
//float target_yaw = -atan2f(aim_select.armor_pose[target_index].x,
// aim_select.armor_pose[target_index].y);
float target_yaw = atan2f(aim_select.armor_pose[target_index].y, aim_select.armor_pose[target_index].x) * 180 / PI;
//float target_yaw = aim_select.armor_pose[target_index].yaw * 180 / PI;
float target_yaw = trajectory_cal.cmd_yaw * 180 / PI;
//float target_yaw = trajectory_cal.cmd_yaw * 180 / PI;
temp_yaw = target_yaw; //for test
float yaw_err = fabsf(target_yaw - gimbal_fetch_data.gimbal_imu_data.Yaw);
if (yaw_err <= 2) //3度
{
aim_select.suggest_fire = 1;
temp_yaw_err = yaw_err;
if(vision_recv_data->armors_num == 3){
if (yaw_err <= 2) //3度
{
aim_select.suggest_fire = 1;
}
else
aim_select.suggest_fire = 0;
}else{
if (yaw_err <= 4) //3度
{
aim_select.suggest_fire = 1;
}
else
aim_select.suggest_fire = 0;
}
else
aim_select.suggest_fire = 0;
// float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw);
// if (yaw_err <= 3) //3度

2
application/robot_def.h
View File

@ -26,7 +26,7 @@
/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
// 云台参数
#define YAW_CHASSIS_ALIGN_ECD 4.386964 // 云台和底盘对齐指向相同方向时的电机position值,需要加上PI，因为单圈角度计算里增加了PI若对云台有机械改动需要修改
#define YAW_CHASSIS_ALIGN_ECD 3.641593 // 云台和底盘对齐指向相同方向时的电机position值,需要加上PI，因为单圈角度计算里增加了PI若对云台有机械改动需要修改
#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
#define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
#define PITCH_MAX_ANGLE 13 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)

73
modules/auto_aim/auto_aim.c
View File

@ -58,43 +58,15 @@ int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_c
aim_sel->armor_pose[i].yaw = aim_sel->target_state.yaw + i * (2.0f * PI / 3.0f);
use_1 = !use_1;
}
// 选择两块较近的装甲板
// 选择较近的装甲板
float distance[3];
int label = 0;
for (i = 0; i < 3; i++) {
distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
if(distance[i]<distance[label])
label = i;
}
int label_first = 0;
int label_second = 1;
if(distance[label_first] > distance[label_second])
{
label_first = 1;
label_second = 0;
}
if(distance[2]<distance[label_second])
label_second = 2;
//再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
float center_length = sqrtf(powf(aim_sel->target_state.x, 2) + powf(aim_sel->target_state.y, 2));
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;
idx = label;
} else {
for (i = 0; i < 4; i++) {
float tmp_yaw = aim_sel->target_state.yaw + i * PI / 2.0f;
@ -253,41 +225,16 @@ int aim_center_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_
// } else label_second = 1;
// }
// 选择两块较近的装甲板
// 选择较近的装甲板
float distance[3];
int label = 0;
for (i = 0; i < 3; i++) {
distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
if(distance[i]<distance[label])
label = i;
}
int label_first = 0;
int label_second = 1;
if(distance[label_first] > distance[label_second])
{
label_first = 1;
label_second = 0;
}
if(distance[2]<distance[label_second])
label_second = 2;
//再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
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;
idx = label;
} else {
for (i = 0; i < 4; i++) {
float tmp_yaw = aim_sel->target_state.yaw + i * PI / 2.0f;