遥控器

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固定角度
@@ -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,16 +393,17 @@ void RobotCMDTask() {
 
     // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成
     CalcOffsetAngle();
-    // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠
 
+    // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠
     static control_mode_e last_gimbal_mode = 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 {
-
-    else {
             if (last_gimbal_mode == AUTO_CONTROL) {
                 gimbal_cmd_send.yaw = 0;
                 gimbal_cmd_send.pitch = 0;
@@ -408,18 +411,20 @@ void RobotCMDTask() {
             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)
         shoot_cmd_send.load_mode = LOAD_STOP;
 
-    EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
+
 
 //    chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color;
 //    // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
@@ -432,8 +437,7 @@ void RobotCMDTask() {
 
     //sentry_state = sentry_behave();
 
-    if(referee_data->GameState.game_progress == 4)
+    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)
@@ -460,40 +464,35 @@ 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)
         behave_flag = 0;
-    else if(referee_data->GameState.game_progress == 4)
+    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)
             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(last_behave_flag != 9)
+        if (last_behave_flag != 9) {
-        {
             recover_start_T = referee_data->GameState.stage_remain_time;
             recover_cnt++;
         }

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);
 }

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;