diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 9994e40..ad6e02e 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -16,7 +16,7 @@ #include "bsp_log.h" // 私有宏,自动将编码器转换成角度值 -#define YAW_ALIGN_ANGLE 150.0F //(YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360 +#define YAW_ALIGN_ANGLE 56.0F //(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 -10.0f //扫描阶段PITCH固定角度 @@ -62,14 +62,14 @@ static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息 static Robot_Status_e robot_state; // 机器人整体工作状态 -static referee_info_t* referee_data; // 用于获取裁判系统的数据 +static referee_info_t *referee_data; // 用于获取裁判系统的数据 static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI void RobotCMDInit() { rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个 vision_recv_data = VisionInit(&huart1); // 视觉通信串口 - referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI + referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); @@ -176,7 +176,7 @@ static void RemoteControlSet() { } - //左侧开关状态为[下],视觉模式 + //左侧开关状态为[下],视觉模式 if (switch_is_down(rc_data[TEMP].rc.switch_left)) { gimbal_cmd_send.control_mode = TEST_CONTROL; trajectory_cal.v0 = 28; //弹速30 @@ -238,14 +238,14 @@ static void RemoteControlSet() { //gimbal_cmd_send.friction_mode = FRICTION_ON; else shoot_cmd_send.friction_mode = FRICTION_OFF; - //gimbal_cmd_send.friction_mode = FRICTION_OFF; + //gimbal_cmd_send.friction_mode = FRICTION_OFF; // 拨弹控制,遥控器固定为一种拨弹模式,可自行选择 if (rc_data[TEMP].rc.dial < -500) shoot_cmd_send.load_mode = LOAD_BURSTFIRE; //gimbal_cmd_send.load_mode = LOAD_BURSTFIRE; else shoot_cmd_send.load_mode = LOAD_STOP; - //gimbal_cmd_send.load_mode = LOAD_STOP; + //gimbal_cmd_send.load_mode = LOAD_STOP; gimbal_cmd_send.shoot_mode = shoot_cmd_send.shoot_mode;//遥控发射指令同步发送给右边 gimbal_cmd_send.load_mode = shoot_cmd_send.load_mode;//遥控发射指令同步发送给右边 @@ -322,7 +322,7 @@ 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 <= 5) //3度 + if (yaw_err <= 5) //3度 { aim_select.suggest_fire = 1; shoot_cmd_send.shoot_mode = SHOOT_ON; @@ -371,6 +371,8 @@ static void EmergencyHandler() { if (switch_is_up(rc_data[TEMP].rc.switch_right)) { robot_state = ROBOT_READY; shoot_cmd_send.shoot_mode = SHOOT_ON; + gimbal_cmd_send.yaw = gimbal_fetch_data.mini_yaw_encode_angle - 44; + gimbal_cmd_send.big_yaw = gimbal_fetch_data.big_yaw_motor_angle; LOGINFO("[CMD] reinstate, robot ready"); } } @@ -391,35 +393,38 @@ void RobotCMDTask() { // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成 CalcOffsetAngle(); + // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠 - static control_mode_e last_gimbal_mode = 0; - if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],自动模式 - { - gimbal_cmd_send.control_mode = AUTO_CONTROL; - AutoControlSet(); - } - else { - if (last_gimbal_mode == AUTO_CONTROL) { - gimbal_cmd_send.yaw = 0; - gimbal_cmd_send.pitch = 0; + + if (robot_state != ROBOT_STOP && rc_data[TEMP].rc.dial < 300) { + if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],自动模式 + { + gimbal_cmd_send.control_mode = AUTO_CONTROL; + AutoControlSet(); + } else { + if (last_gimbal_mode == AUTO_CONTROL) { + gimbal_cmd_send.yaw = 0; + gimbal_cmd_send.pitch = 0; + } + gimbal_cmd_send.control_mode = RC_CONTROL; + RemoteControlSet(); } - gimbal_cmd_send.control_mode = RC_CONTROL; - RemoteControlSet(); } + EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况 + gimbal_cmd_send.enemy_color = !referee_data->referee_id.Robot_Color; gimbal_cmd_send.rest_heat = referee_data->PowerHeatData.shooter_17mm_2_barrel_heat; - last_gimbal_mode = gimbal_cmd_send.control_mode; //热量控制 - if(referee_data->PowerHeatData.shooter_17mm_1_barrel_heat>350) + if (referee_data->PowerHeatData.shooter_17mm_1_barrel_heat > 350) shoot_cmd_send.load_mode = LOAD_STOP; - EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况 + // chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color; // // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况 @@ -432,11 +437,10 @@ void RobotCMDTask() { //sentry_state = sentry_behave(); - if(referee_data->GameState.game_progress == 4) - { - if(referee_data->GameState.stage_remain_time >= (300-60)) + if (referee_data->GameState.game_progress == 4) { + if (referee_data->GameState.stage_remain_time >= (300 - 60)) sentry_state = 1;//前一分钟 - else if(referee_data->GameState.stage_remain_time <= 60) + else if (referee_data->GameState.stage_remain_time <= 60) sentry_state = 3;//后一分钟 else sentry_state = 2;//中间阶段 @@ -444,7 +448,7 @@ void RobotCMDTask() { gimbal_cmd_send.game_state = sentry_state; - VisionSetFlag(!referee_data->referee_id.Robot_Color,sentry_state,referee_data->GameRobotState.current_HP); + VisionSetFlag(!referee_data->referee_id.Robot_Color, sentry_state, referee_data->GameRobotState.current_HP); // 推送消息,双板通信,视觉通信等 // 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置 #ifdef ONE_BOARD @@ -460,45 +464,40 @@ void RobotCMDTask() { VisionSend(&vision_send_data); } -uint8_t sentry_behave() -{ +uint8_t sentry_behave() { static uint8_t behave_flag = 0; static uint8_t last_behave_flag = 0; static uint8_t recover_cnt = 0; // 比赛阶段 0准备阶段 1前1分钟 2中间阶段 3后一分钟 4比赛结束 9补给区回血 10保留（无响应） - if(referee_data->GameState.game_progress < 4) + if (referee_data->GameState.game_progress < 4) behave_flag = 0; - else if(referee_data->GameState.game_progress == 4) - { - if(referee_data->GameState.stage_remain_time >= (300-60)) + else if (referee_data->GameState.game_progress == 4) { + if (referee_data->GameState.stage_remain_time >= (300 - 60)) behave_flag = 1;//前一分钟 - else if(referee_data->GameState.stage_remain_time <= 60) + else if (referee_data->GameState.stage_remain_time <= 60) behave_flag = 3;//后一分钟 else behave_flag = 2;//中间阶段 - } - else if (referee_data->GameState.game_progress == 5) + } else if (referee_data->GameState.game_progress == 5) behave_flag = 4; - if(referee_data->GameRobotState.current_HP < 200 && referee_data->GameState.stage_remain_time >= 60 && recover_cnt <= 1) - { + if (referee_data->GameRobotState.current_HP < 200 && referee_data->GameState.stage_remain_time >= 60 && + recover_cnt <= 1) { behave_flag = 9; } - static uint16_t last_HP = 600; static uint16_t recover_start_T = 0; - if(behave_flag == 9 ) //回去回血了 + if (behave_flag == 9) //回去回血了 { - if(last_behave_flag != 9) - { + if (last_behave_flag != 9) { recover_start_T = referee_data->GameState.stage_remain_time; - recover_cnt ++ ; + recover_cnt++; } - if((referee_data->GameState.stage_remain_time - recover_start_T)>10) + if ((referee_data->GameState.stage_remain_time - recover_start_T) > 10) behave_flag = 10;//随便给一个值 } diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index c76afe7..bbcc1d1 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -278,7 +278,7 @@ void GimbalTask() //loop_float_constrain(big_yaw_relative_angle,0,2*PI) * RAD_2_DEGREE; gimbal_feedback_data.mini_yaw_encode_angle = yaw_motor->measure.total_angle; gimbal_feedback_data.big_yaw_online = DMMotorIsOnline(big_yaw_motor); - + gimbal_feedback_data.big_yaw_motor_angle = big_yaw_motor->measure.total_angle; // 推送消息 PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data); } diff --git a/application/robot_def.h b/application/robot_def.h index 9dc2f92..deb1e69 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -224,6 +224,7 @@ typedef struct attitude_t gimbal_imu_data; float yaw_motor_single_round_angle; float mini_yaw_encode_angle; + float big_yaw_motor_angle; uint8_t big_yaw_online; } Gimbal_Upload_Data_s;