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

自瞄上操作端并优化，pitch修正失败，ui刷新完成

This commit is contained in:
zcj 2024-03-23 07:44:19 +08:00
parent 507cdf96cb
commit 2b402788d1
7 changed files with 339 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
application/chassis/chassis.c
View File

@ -160,10 +160,10 @@ void ChassisInit()
*/
static void MecanumCalculate()
{
vt_lf = chassis_vx + chassis_vy - chassis_cmd_recv.wz * (LF_CENTER-1);
vt_rf = -chassis_vx + chassis_vy + chassis_cmd_recv.wz * (RF_CENTER-1);
vt_lb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * (-LB_CENTER-1);
vt_rb = -chassis_vx - chassis_vy + chassis_cmd_recv.wz * (-RB_CENTER-1);
vt_lf = chassis_vx + chassis_vy - chassis_cmd_recv.wz * (LF_CENTER);
vt_rf = -chassis_vx + chassis_vy + chassis_cmd_recv.wz * (RF_CENTER);
vt_lb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * (-LB_CENTER);
vt_rb = -chassis_vx - chassis_vy + chassis_cmd_recv.wz * (-RB_CENTER);
}
@ -255,13 +255,13 @@ void ChassisTask()
break;
case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
chassis_cmd_recv.wz = 2.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
if(chassis_cmd_recv.wz > 2000)
chassis_cmd_recv.wz = 2000;
if(chassis_cmd_recv.wz < -2000)
chassis_cmd_recv.wz = -2000;
if(chassis_cmd_recv.wz > 3000)
chassis_cmd_recv.wz = 3000;
if(chassis_cmd_recv.wz < -3000)
chassis_cmd_recv.wz = -3000;
break;
case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
chassis_cmd_recv.wz = 2300;
chassis_cmd_recv.wz = 3000;
break;
default:
break;

171
application/cmd/robot_cmd.c
View File

@ -133,9 +133,61 @@ static void pitch_limit()
// gimbal_cmd_send.pitch -= 0.1f;
// if(gimbal_fetch_data.pitch_motor_total_angle < horizontal_angle - 4000)
// gimbal_cmd_send.pitch += 0.1f;
if(gimbal_cmd_send.pitch>18) gimbal_cmd_send.pitch = 18;
if(gimbal_cmd_send.pitch<-38) gimbal_cmd_send.pitch = -38;
if(gimbal_cmd_send.pitch>18) gimbal_cmd_send.pitch = 18;
if(gimbal_cmd_send.pitch<-38) gimbal_cmd_send.pitch = -38;
}
static void hand_aim_mode()
{
gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 9; // 系数待测
gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 9;
pitch_limit();
}
static void auto_aim_mode()
{
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;
target_index = auto_aim(&aim_select, &trajectory_cal, vision_recv_data);
VisionSetAim(aim_select.aim_point[0],aim_select.aim_point[1],aim_select.aim_point[2]);
gimbal_cmd_send.yaw = -trajectory_cal.cmd_yaw * 180 / PI;
gimbal_cmd_send.pitch = -trajectory_cal.cmd_pitch * 180 / PI;
float target_yaw = atan2f(aim_select.armor_pose[target_index].x,
aim_select.armor_pose[target_index].y);
float yaw_err = fabsf(target_yaw - gimbal_fetch_data.gimbal_imu_data.Yaw);
if (yaw_err <= 3) //3度
{
aim_select.suggest_fire = 1;
}
else
aim_select.suggest_fire = 0;
// float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw);
// if (yaw_err <= 3) //3度
// aim_select.suggest_fire = 1;
// else
// aim_select.suggest_fire = 0;
}
}
/**
* @brief 控制输入为遥控器(调试时)的模式和控制量设置
*
@ -169,45 +221,7 @@ static void RemoteControlSet()
}
// 左侧开关状态为[下],视觉模式
if (switch_is_down(rc_data[TEMP].rc.switch_left)) {
trajectory_cal.v0 = 12.5; //弹速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;
target_index = auto_aim(&aim_select, &trajectory_cal, vision_recv_data);
VisionSetAim(aim_select.aim_point[0],aim_select.aim_point[1],aim_select.aim_point[2]);
gimbal_cmd_send.yaw = -trajectory_cal.cmd_yaw * 180 / PI;
gimbal_cmd_send.pitch = -trajectory_cal.cmd_pitch * 180 / PI;
float target_yaw = atan2f(aim_select.armor_pose[target_index].x,
aim_select.armor_pose[target_index].y);
float yaw_err = fabsf(target_yaw - gimbal_fetch_data.gimbal_imu_data.Yaw);
if (yaw_err <= 3) //3度
aim_select.suggest_fire = 1;
else
aim_select.suggest_fire = 0;
// float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw);
// if (yaw_err <= 3) //3度
// aim_select.suggest_fire = 1;
// else
// aim_select.suggest_fire = 0;
}
auto_aim_mode();
}
// 云台软件限位
@ -242,12 +256,10 @@ static void RemoteControlSet()
*/
static void MouseKeySet()
{
chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 6000 - rc_data[TEMP].key[KEY_PRESS].d * 6000; // 系数待测
chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 6000 - rc_data[TEMP].key[KEY_PRESS].s * 6000;
chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 20000 - rc_data[TEMP].key[KEY_PRESS].d * 20000; // 系数待测
chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 20000 - rc_data[TEMP].key[KEY_PRESS].s * 20000;
gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 9; // 系数待测
gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 9;
pitch_limit();
// switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速//
// {
// case 0:
@ -260,11 +272,37 @@ static void MouseKeySet()
// shoot_cmd_send.bullet_speed = 30;
// break;
// }
switch (rc_data[TEMP].mouse.press_r) // 右键自瞄模式
{
case 1:
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))
hand_aim_mode();
else
auto_aim_mode();
break;
default:
hand_aim_mode();
break;
}
switch (rc_data[TEMP].mouse.press_l) // 左键发射模式
{
case 1:
if(shoot_cmd_send.friction_mode == FRICTION_ON)
{
if(rc_data[TEMP].mouse.press_r && rc_data[TEMP].mouse.press_l)
{
if(aim_select.suggest_fire == 1)
shoot_cmd_send.load_mode = LOAD_1_BULLET;
else
shoot_cmd_send.load_mode = LOAD_STOP;
break;
}
shoot_cmd_send.load_mode = LOAD_1_BULLET;
}
else
shoot_cmd_send.load_mode = LOAD_STOP;
break;
@ -272,13 +310,14 @@ static void MouseKeySet()
shoot_cmd_send.load_mode = LOAD_STOP;
break;
}
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键开关弹舱
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键手动刷新UI
{
case 0:
shoot_cmd_send.lid_mode = LID_OPEN;
case 1:
MyUIInit();
rc_data[TEMP].key_count[KEY_PRESS][Key_R]++;
break;
default:
shoot_cmd_send.lid_mode = LID_CLOSE;
break;
}
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮
@ -290,7 +329,7 @@ static void MouseKeySet()
shoot_cmd_send.friction_mode = FRICTION_ON;
break;
}
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q键开关小陀螺
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺
{
case 0:
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
@ -301,21 +340,21 @@ static void MouseKeySet()
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
break;
}
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_C] % 4) // C键设置底盘速度
{
case 0:
chassis_cmd_send.chassis_speed_buff = 40;
break;
case 1:
chassis_cmd_send.chassis_speed_buff = 60;
break;
case 2:
chassis_cmd_send.chassis_speed_buff = 80;
break;
default:
chassis_cmd_send.chassis_speed_buff = 100;
break;
}
// switch (rc_data[TEMP].key_count[KEY_PRESS][Key_C] % 4) // C键设置底盘速度
// {
// case 0:
// chassis_cmd_send.chassis_speed_buff = 40;
// break;
// case 1:
// chassis_cmd_send.chassis_speed_buff = 60;
// break;
// case 2:
// chassis_cmd_send.chassis_speed_buff = 80;
// break;
// default:
// chassis_cmd_send.chassis_speed_buff = 100;
// break;
// }
switch (rc_data[TEMP].key[KEY_PRESS].shift) // 待添加 按shift允许超功率 消耗缓冲能量
{
case 1:

15
application/gimbal/gimbal.c
View File

@ -27,10 +27,9 @@ void GimbalInit()
},
.controller_param_init_config = {
.angle_PID = {
.Kp = 40, // 8
.Kp = 200, // 8
.Ki = 0,
.Kd = 5,
.DeadBand = 0.1,
.Kd = 20,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 100,
@ -38,8 +37,8 @@ void GimbalInit()
},
.speed_PID = {
.Kp = 70, // 70
.Ki = 200, // 200
.Kd = 10,//10
.Ki = 0, // 200
.Kd = 0,//10
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 3000,
.MaxOut = 20000,
@ -65,8 +64,8 @@ void GimbalInit()
},
.controller_param_init_config = {
.angle_PID = {
.Kp = 15, // 10
.Ki = 0.1f,
.Kp = 20, // 65
.Ki = 0,
.Kd = 1,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 100,//100
@ -75,7 +74,7 @@ void GimbalInit()
.speed_PID = {
.Kp = 30, // 50
.Ki = 0, // 350
.Kd = 1, // 0
.Kd = 2, // 0
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 150,//2500
.MaxOut = 2000,//20000

20
application/shoot/shoot.c
View File

@ -55,15 +55,15 @@ void ShootInit()
.outer_loop_type = SPEED_LOOP,
.close_loop_type = SPEED_LOOP ,
.motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
},
.motor_type = M3508};
friction_config.can_init_config.tx_id = 3,
friction_l = DJIMotorInit(&friction_config);
friction_config.can_init_config.tx_id = 4; // 右摩擦轮,改txid和方向就行
friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
friction_r = DJIMotorInit(&friction_config);
// friction_config.can_init_config.tx_id = 4; // 右摩擦轮,改txid和方向就行
// friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
// friction_r = DJIMotorInit(&friction_config);
friction_config.can_init_config.tx_id = 6; // 上摩擦轮,改txid和方向就行
friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
@ -135,14 +135,14 @@ void ShootTask()
if (shoot_cmd_recv.shoot_mode == SHOOT_OFF)
{
DJIMotorStop(friction_l);
DJIMotorStop(friction_r);
// DJIMotorStop(friction_r);
DJIMotorStop(friction_z);
DJIMotorStop(loader);
}
else // 恢复运行
{
DJIMotorEnable(friction_l);
DJIMotorEnable(friction_r);
// DJIMotorEnable(friction_r);
DJIMotorEnable(friction_z);
DJIMotorEnable(loader);
}
@ -193,14 +193,14 @@ void ShootTask()
// 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启)
if (shoot_cmd_recv.friction_mode == FRICTION_ON)
{
DJIMotorSetRef(friction_l, 39000);
DJIMotorSetRef(friction_r, 39000);
DJIMotorSetRef(friction_z, 39000);//39000/6 = 6500
DJIMotorSetRef(friction_l, 36000);
// DJIMotorSetRef(friction_r, 39000);
DJIMotorSetRef(friction_z, 36000);//39000/6 = 6500
}
else // 关闭摩擦轮
{
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0);
// DJIMotorSetRef(friction_r, 0);
DJIMotorSetRef(friction_z, 0);
}

207
modules/auto_aim/auto_aim.c
View File

@ -3,14 +3,203 @@
//
#include "auto_aim.h"
#include "arm_math.h"
uint8_t mode_flag=0;
/**
* @brief 选择目标装甲板（瞄装甲板）
* @author SJQ
* @param[in] trajectory_cal:弹道解算结构体
* @retval 返回空
*/
int 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;
// }
// 平衡步兵选择两块装甲板中较近的装甲板
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++) {
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;
// }
// }
// 选择两块较近的装甲板，再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
// 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++) {
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;
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;
// }
// }
// 选择两块较近的装甲板
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++) {
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;
aim_sel->aim_point[2] = aim_sel->armor_pose[idx].z + aim_sel->target_state.vz * aim_sel->delay_time;
return idx;
}
/**
* @brief 选择目标装甲板
* @author SJQ
* @param[in] trajectory_cal:弹道解算结构体
* @retval 返回空
*/
int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal) {
int aim_center_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;
@ -229,7 +418,7 @@ 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->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]);
@ -264,7 +453,7 @@ void get_cmd_angle(Trajectory_Type_t *trajectory_cal) {
}
int auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, RecievePacket_t *receive_packet) {
trajectory_cal->extra_delay_time = 0.025f;//0.025
trajectory_cal->extra_delay_time = 0.035f;//0.025
aim_sel->target_state.armor_type = receive_packet->id;
aim_sel->target_state.armor_num = receive_packet->armors_num;
@ -280,7 +469,17 @@ 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.dz = receive_packet->dz;
int idx = aim_armor_select(aim_sel, trajectory_cal);
int idx = 0;
if(aim_sel->target_state.v_yaw >= 0.7* PI)
{
mode_flag=0;
idx = aim_center_select(aim_sel, trajectory_cal);
}
else
{
mode_flag=1;
idx = 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];

8
modules/referee/referee_task.c
View File

@ -62,8 +62,8 @@ static uint32_t shoot_line_location[10] = {540, 960, 490, 515, 565};
void MyUIInit()
{
// if (!referee_recv_info->init_flag)
// vTaskDelete(NULL); // 如果没有初始化裁判系统则直接删除ui任务
if (!referee_recv_info->init_flag)
vTaskDelete(NULL); // 如果没有初始化裁判系统则直接删除ui任务
while (referee_recv_info->GameRobotState.robot_id == 0)
osDelay(100); // 若还未收到裁判系统数据,等待一段时间后再检查
@ -94,9 +94,9 @@ void MyUIInit()
// 绘制车辆状态标志，动态
// 由于初始化时xxx_last_mode默认为0，所以此处对应UI也应该设为0时对应的UI，防止模式不变的情况下无法置位flag，导致UI无法刷新
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce");
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 270, 750, "follow");
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]);
UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce");
UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 270, 700, "free");
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
UICharDraw(&UI_State_dyn[2], "sd2", UI_Graph_ADD, 8, UI_Color_Orange, 15, 2, 270, 650, "off");
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[2]);

1
modules/referee/rm_referee.h
View File

@ -68,6 +68,7 @@ typedef struct
lid_mode_e lid_mode; // 弹舱盖打开
Chassis_Power_Data_s Chassis_Power_Data; // 功率控制
// 上一次的模式，用于flag判断
chassis_mode_e chassis_last_mode;
gimbal_mode_e gimbal_last_mode;