diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 49632ce..ffc07bf 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -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(); // 处理模块离线和遥控器急停等紧急情况 diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index cbf09f0..9cce0e7 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -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; diff --git a/application/robot_def.h b/application/robot_def.h index 1d846fc..bad00d1 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -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 diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index 0584888..8d0b7a2 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -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; } } diff --git a/modules/auto_aim/auto_aim.c b/modules/auto_aim/auto_aim.c index a4badb2..f8bd400 100644 --- a/modules/auto_aim/auto_aim.c +++ b/modules/auto_aim/auto_aim.c @@ -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_second]) - { - label_first = 1; - label_second = 0; - } - - if(distance[2]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;