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 21:54:02 +08:00
parent e78845b802
commit 51982171c2
4 changed files with 150 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
application/gimbal/gimbal.c
View File

@ -4,8 +4,8 @@
#include "ins_task.h"
#include "message_center.h"
#include "general_def.h"
#include "bmi088.h"
#include "vofa.h"
static attitude_t *gimba_IMU_data; // 云台IMU数据
static DJIMotorInstance *yaw_motor, *pitch_motor;
@ -14,9 +14,32 @@ static Publisher_t *gimbal_pub; // 云台应用消息发布者
static Subscriber_t *gimbal_sub; // cmd控制消息订阅者
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息
#include "vofa.h"
static void LimitPitchAngleAuto() {
/** 注意电机角度与陀螺仪角度方向是否相同
* 目前 add > 0, 向下转动
* 电机角度值减小
* 陀螺仪角度值增大
**/
float add;
float angle_set;
add = gimbal_cmd_recv.pitch - gimba_IMU_data->Pitch;
if(pitch_motor->measure.angle_single_round - add > PITCH_MAX_RELATIVE_ANGLE){
if(add < 0.0f ){
add = PITCH_MAX_ANGLE - pitch_motor->measure.angle_single_round;
}
}else if(pitch_motor->measure.angle_single_round - add < PITCH_MIN_RELATIVE_ANGLE){
if(add > 0.0f){
add = PITCH_MIN_RELATIVE_ANGLE - pitch_motor->measure.angle_single_round;
}
}
angle_set = gimba_IMU_data->Pitch;
DJIMotorSetRef(pitch_motor, angle_set+add);
}
void GimbalInit() {
gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
// YAW
@ -127,6 +150,7 @@ void GimbalTask() {
DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
//LimitPitchAngleAuto();
break;
// 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关
case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快)

5
application/robot_def.h
View File

@ -31,6 +31,11 @@
#define PITCH_HORIZON_ECD 1670 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
#define PITCH_MAX_ANGLE 25 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
#define PITCH_MIN_ANGLE -18 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
#define PITCH_MAX_RELATIVE_ANGLE 123 // 云台相对底盘最大角度
#define PITCH_MIN_RELATIVE_ANGLE 80 // 云台相对底盘最小角度
// 发射参数
#define ONE_BULLET_DELTA_ANGLE 45 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f

4
application/shoot/shoot.c
View File

@ -217,8 +217,8 @@ void ShootTask()
// DJIMotorSetRef(friction_r, 0);
// break;
//default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
DJIMotorSetRef(friction_l, 30000);
DJIMotorSetRef(friction_r, 30000);
DJIMotorSetRef(friction_l, 34000);
DJIMotorSetRef(friction_r, 34000);
// break;
// }
}

135
modules/auto_aim/auto_aim.c
View File

@ -30,14 +30,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;
}
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;
// 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;
// }
// 平衡步兵选择两块装甲板中较近的装甲板
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;
}
} else if (aim_sel->target_state.armor_num == 3)//前哨站
{
for (i = 0; i < 3; i++) {
@ -55,15 +63,70 @@ void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_
use_1 = !use_1;
}
//计算枪管到目标装甲板yaw最小的那个装甲板
float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
// //计算枪管到目标装甲板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++) {
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;
if(distance[i] < distance[label_first]){
label_second = label_first;
label_first = 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 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;
} else {
for (i = 0; i < 4; i++) {
float tmp_yaw = aim_sel->target_state.yaw + i * PI / 2.0;
@ -77,15 +140,51 @@ void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_
use_1 = !use_1;
}
//计算枪管到目标装甲板yaw最小的那个装甲板
float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
// //计算枪管到目标装甲板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++) {
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;
if(distance[i] < distance[label_first]){
label_second = label_first;
label_first = 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 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;
}
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;