2024超级对抗赛版本
This commit is contained in:
parent
780782f6ec
commit
6cdd3d046d
|
|
@ -17,10 +17,10 @@
|
|||
#define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360
|
||||
#define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF_DJI) // pitch水平时电机的角度,0-360
|
||||
|
||||
#define PITCH_SCAN -8.0f //扫描阶段PITCH固定角度
|
||||
|
||||
#define SHOOT_RATE 18.0f //射频
|
||||
#define PITCH_SCAN -10.0f //扫描阶段PITCH固定角度
|
||||
|
||||
#define SHOOT_RATE 15.0f //射频
|
||||
#define MIN_SHOOT_RATE 5.0f //热量过高降低射频射频
|
||||
/* cmd应用包含的模块实例指针和交互信息存储*/
|
||||
#ifdef GIMBAL_BOARD // 对双板的兼容,条件编译
|
||||
|
||||
|
|
@ -60,8 +60,6 @@ static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息
|
|||
|
||||
static Robot_Status_e robot_state; // 机器人整体工作状态
|
||||
|
||||
float pitch_k = 0.13;
|
||||
|
||||
void RobotCMDInit() {
|
||||
rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
|
||||
vision_recv_data = VisionInit(&huart1); // 视觉通信串口
|
||||
|
|
@ -172,28 +170,29 @@ static void AutoControlSet() {
|
|||
static int8_t yaw_dir = 1;
|
||||
static int8_t pitch_dir = 1;
|
||||
//trajectory_cal.v0 = 30; //弹速30
|
||||
aim_select.suggest_fire = 0;
|
||||
|
||||
//小云台扫描
|
||||
if (gimbal_scan_flag == 1) {
|
||||
|
||||
gimbal_cmd_send.pitch = PITCH_SCAN;
|
||||
//if(gimbal_cmd_recv.game_state > 1)
|
||||
{
|
||||
if (yaw_dir == 1) gimbal_cmd_send.yaw += 0.06f;
|
||||
else gimbal_cmd_send.yaw -= 0.06f;
|
||||
if (gimbal_fetch_data.mini_yaw_encode_angle <= YAW_MIN_ENCODE_ANGLE + 2.0f) yaw_dir = 1;
|
||||
if (gimbal_fetch_data.mini_yaw_encode_angle >= YAW_MAX_ENCODE_ANGLE - 2.0f) yaw_dir = -1;
|
||||
}
|
||||
|
||||
if (yaw_dir == 1) gimbal_cmd_send.yaw -= 0.06f;
|
||||
else gimbal_cmd_send.yaw += 0.06f;
|
||||
if (gimbal_fetch_data.mini_yaw_encode_angle <= YAW_MIN_ENCODE_ANGLE + 2.0f) yaw_dir = -1;
|
||||
if (gimbal_fetch_data.mini_yaw_encode_angle >= YAW_MAX_ENCODE_ANGLE - 2.0f) yaw_dir = 1;
|
||||
|
||||
shoot_cmd_send.load_mode = LOAD_STOP;
|
||||
}
|
||||
|
||||
trajectory_cal.v0 = 28; //弹速30
|
||||
trajectory_cal.v0 = 27; //弹速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 >= 200) {
|
||||
if (no_find_cnt >= 400) {
|
||||
gimbal_scan_flag = 1;
|
||||
//auto_aim_flag = 0;
|
||||
}
|
||||
|
|
@ -212,10 +211,9 @@ static void AutoControlSet() {
|
|||
|
||||
gimbal_cmd_send.pitch = trajectory_cal.cmd_pitch * 180 / PI;
|
||||
|
||||
|
||||
float yaw_err = fabsf(gimbal_cmd_send.yaw - gimbal_fetch_data.gimbal_imu_data.Yaw);
|
||||
|
||||
if (yaw_err <= 3) //3度
|
||||
if (yaw_err <= 5) //3度
|
||||
{
|
||||
aim_select.suggest_fire = 1;
|
||||
shoot_cmd_send.shoot_mode = SHOOT_ON;
|
||||
|
|
@ -397,7 +395,7 @@ static void EmergencyHandler() {
|
|||
LOGINFO("[CMD] reinstate, robot ready");
|
||||
}
|
||||
}
|
||||
|
||||
static uint8_t cool_down;
|
||||
/* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */
|
||||
void RobotCMDTask() {
|
||||
// 从其他应用获取回传数据
|
||||
|
|
@ -427,8 +425,15 @@ void RobotCMDTask() {
|
|||
}
|
||||
//热量控制
|
||||
|
||||
if (gimbal_cmd_recv.rest_heat > 350)
|
||||
shoot_cmd_send.load_mode = LOAD_STOP;
|
||||
if (gimbal_cmd_recv.rest_heat > 350){
|
||||
cool_down = 1; //进入冷却状态
|
||||
}else if(gimbal_cmd_recv.rest_heat < 100){
|
||||
cool_down = 0; //退出冷却状态
|
||||
}
|
||||
if(cool_down){
|
||||
shoot_cmd_send.shoot_rate = MIN_SHOOT_RATE;
|
||||
}
|
||||
|
||||
|
||||
EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
|
||||
|
||||
|
|
|
|||
|
|
@ -206,7 +206,7 @@ void GimbalTask()
|
|||
// 设置反馈数据,主要是imu和yaw的ecd
|
||||
gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
|
||||
gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round;
|
||||
gimbal_feedback_data.mini_yaw_encode_angle = yaw_motor->measure.total_angle;
|
||||
gimbal_feedback_data.mini_yaw_encode_angle = yaw_motor->measure.angle_single_round;
|
||||
|
||||
big_yaw_angle = gimba_IMU_data->YawTotalAngle - (yaw_motor->measure.total_angle - (-100));
|
||||
gimbal_feedback_data.big_yaw_angle = big_yaw_angle;
|
||||
|
|
|
|||
|
|
@ -32,8 +32,8 @@
|
|||
#define PITCH_MAX_ANGLE 35 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
|
||||
#define PITCH_MIN_ANGLE -15 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
|
||||
|
||||
#define YAW_MAX_ENCODE_ANGLE 45.0f //小yaw编码器限位
|
||||
#define YAW_MIN_ENCODE_ANGLE -77.0f
|
||||
#define YAW_MAX_ENCODE_ANGLE 300.0f //小yaw编码器限位
|
||||
#define YAW_MIN_ENCODE_ANGLE 130.0f
|
||||
// 发射参数
|
||||
#define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
|
||||
#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
|
||||
|
|
|
|||
|
|
@ -221,8 +221,8 @@ void ShootTask()
|
|||
// DJIMotorSetRef(friction_r, 0);
|
||||
// break;
|
||||
default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
|
||||
DJIMotorSetRef(friction_l, 43000);
|
||||
DJIMotorSetRef(friction_r, 43000);
|
||||
DJIMotorSetRef(friction_l, 40000);
|
||||
DJIMotorSetRef(friction_r, 40000);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -91,41 +91,16 @@ int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_c
|
|||
// } 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 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++) {
|
||||
|
|
@ -212,7 +187,7 @@ int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_c
|
|||
* @param[in] v_x0:弹速水平分量
|
||||
* @retval 返回空
|
||||
*/
|
||||
const float k1 = 0.005f;//0.0761; //标准大气压25度下空气阻力系数
|
||||
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;
|
||||
|
|
@ -235,7 +210,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.50f;
|
||||
trajectory_cal->dis = trajectory_cal->dis - 0.05f;
|
||||
|
||||
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]);
|
||||
|
|
@ -270,7 +245,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.035;//0.025
|
||||
trajectory_cal->extra_delay_time = 0.09;//0.025
|
||||
|
||||
aim_sel->target_state.armor_type = receive_packet->id;
|
||||
aim_sel->target_state.armor_num = receive_packet->armors_num;
|
||||
|
|
|
|||
Loading…
Reference in New Issue