修改训练赛的问题

application/chassis/chassis.c

@@ -43,7 +43,7 @@ static Subscriber_t *chassis_sub;                   // 用于订阅底盘的控
 static Chassis_Ctrl_Cmd_s chassis_cmd_recv;         // 底盘接收到的控制命令
 static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据
 
-static referee_info_t* referee_data; // 用于获取裁判系统的数据
+
 
 
 static SuperCapInstance *cap;                                       // 超级电容
@@ -110,7 +110,7 @@ void ChassisInit()
     chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
     motor_rb = DJIMotorInit(&chassis_motor_config);
 
-    referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI
+
 
     SuperCap_Init_Config_s cap_conf = {
         .can_config = {
@@ -289,7 +289,7 @@ void ChassisTask()
 
     // // 获取裁判系统数据   建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
     // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
-    chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color;
+    //chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color;
 
     // // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
     // chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit;

application/cmd/robot_cmd.c

@@ -27,6 +27,8 @@ static CANCommInstance *cmd_can_comm; // 双板通信
 static Publisher_t *chassis_cmd_pub;   // 底盘控制消息发布者
 static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者
 #endif                                 // ONE_BOARD
+static float temp_yaw; //for test
+static float temp_index; //for test
 
 static Chassis_Ctrl_Cmd_s chassis_cmd_send;      // 发送给底盘应用的信息,包括控制信息和UI绘制相关
 static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等
@@ -54,8 +56,9 @@ static Shoot_Ctrl_Cmd_s shoot_cmd_send;      // 传递给发射的控制信息
 static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息
 
 static Robot_Status_e robot_state; // 机器人整体工作状态
+static referee_info_t* referee_data; // 用于获取裁判系统的数据
 extern float horizontal_angle;
-int target_index = -1;
+static int target_index = -1;
 
 void RobotCMDInit()
 {
@@ -83,6 +86,7 @@ void RobotCMDInit()
     };
     cmd_can_comm = CANCommInit(&comm_conf);
 #endif // GIMBAL_BOARD
+    referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI
     gimbal_cmd_send.pitch = 0;
 
     robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入
@@ -119,6 +123,15 @@ static void CalcOffsetAngle()
 //        chassis_cmd_send.offset_angle = chassis_cmd_send.offset_angle + 360.0f;
 //    else
 //        chassis_cmd_send.offset_angle = chassis_cmd_send.offset_angle;
+}
+static void death_check()
+{
+    if(referee_data->GameRobotState.remain_HP <= 0)
+    {
+        chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
+        gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
+    }
+
 }
 static void update_ui_data()
 {
@@ -136,15 +149,9 @@ static void pitch_limit()
     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
+    trajectory_cal.v0 = 15.8f; //弹速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;
@@ -163,14 +170,27 @@ static void auto_aim_mode()
         auto_aim_flag = 1;
 
         target_index = auto_aim(&aim_select, &trajectory_cal, vision_recv_data);
+        temp_index = target_index;
         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;
+        float single_angle_yaw_now = gimbal_fetch_data.gimbal_imu_data.Yaw;
+        float diff_yaw = trajectory_cal.cmd_yaw * 180 / PI - single_angle_yaw_now;
+        if(diff_yaw>180)
+            diff_yaw -= 360;
+        else if(diff_yaw<-180)
+            diff_yaw += 360;
+
+        gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle + diff_yaw;
 
         gimbal_cmd_send.pitch = -trajectory_cal.cmd_pitch * 180 / PI;
 
-        float target_yaw = atan2f(aim_select.armor_pose[target_index].x,
+//        float target_yaw = -atan2f(aim_select.armor_pose[target_index].x,
-                                  aim_select.armor_pose[target_index].y);
+//                                  aim_select.armor_pose[target_index].y);
+
+        //float target_yaw = aim_select.armor_pose[target_index].yaw;
+        float target_yaw = trajectory_cal.cmd_yaw * 180 / PI;
+        temp_yaw = target_yaw; //for test
+
         float yaw_err = fabsf(target_yaw - gimbal_fetch_data.gimbal_imu_data.Yaw);
         if (yaw_err <= 3) //3度
         {
@@ -214,7 +234,7 @@ static void RemoteControlSet()
          vision_recv_data->vz == 0)) // 左侧开关状态为[中],或视觉未识别到目标,纯遥控器拨杆控制
 
     {
-        gimbal_cmd_send.yaw += 0.003f * (float)rc_data[TEMP].rc.rocker_l_;
+        gimbal_cmd_send.yaw -= 0.003f * (float)rc_data[TEMP].rc.rocker_l_;
         gimbal_cmd_send.pitch += 0.0007f * (float)rc_data[TEMP].rc.rocker_l1;
         pitch_limit();
 
@@ -259,6 +279,41 @@ static void MouseKeySet()
     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 *5 ;
+
+
+    if (rc_data[TEMP].mouse.press_l && (!rc_data[TEMP].mouse.press_r)) // 左键发射模式
+    {
+        if (shoot_cmd_send.friction_mode == FRICTION_ON) {
+            shoot_cmd_send.load_mode = LOAD_1_BULLET;
+        }
+    } else if ((!rc_data[TEMP].mouse.press_l) && (!rc_data[TEMP].mouse.press_r))
+    {
+        shoot_cmd_send.load_mode = LOAD_STOP;
+
+    }
+    if (rc_data[TEMP].mouse.press_r) // 右键自瞄模式
+    {
+
+        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))
+        {
+            shoot_cmd_send.load_mode = LOAD_STOP;
+        }
+
+
+        else {
+            auto_aim_mode();
+            if (aim_select.suggest_fire == 1 && rc_data[TEMP].mouse.press_l &&
+                shoot_cmd_send.friction_mode == FRICTION_ON) {
+                shoot_cmd_send.load_mode = LOAD_1_BULLET;
+            } else {
+                shoot_cmd_send.load_mode = LOAD_STOP;
+            }
+        }
+    }
 
 //    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速//
 //    {
@@ -273,43 +328,44 @@ static void MouseKeySet()
 //        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) // 左键发射模式
+//    switch (rc_data[TEMP].mouse.press_r) // 右键自瞄模式
-    {
+//    {
-    case 1:
+//        case 1:
-        if(shoot_cmd_send.friction_mode == FRICTION_ON)
+//            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 &&
-            if(rc_data[TEMP].mouse.press_r && rc_data[TEMP].mouse.press_l)
+//                vision_recv_data->vz == 0))
-            {
+//                hand_aim_mode();
-                if(aim_select.suggest_fire == 1)
+//            else
-                    shoot_cmd_send.load_mode = LOAD_1_BULLET;
+//                auto_aim_mode();
-                else
+//            break;
-                    shoot_cmd_send.load_mode = LOAD_STOP;
+//        default:
-                break;
+//            hand_aim_mode();
-            }
+//            break;
-            shoot_cmd_send.load_mode = LOAD_1_BULLET;
+//    }
-        }
+//
-        else
+//    switch (rc_data[TEMP].mouse.press_l) // 左键发射模式
-            shoot_cmd_send.load_mode = LOAD_STOP;
+//    {
-        break;
+//    case 1:
-    default:
+//        if(shoot_cmd_send.friction_mode == FRICTION_ON)
-        shoot_cmd_send.load_mode = LOAD_STOP;
+//        {
-        break;
+//            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;
+//    default:
+//        shoot_cmd_send.load_mode = LOAD_STOP;
+//        break;
+//    }
 
     switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键手动刷新UI
     {
@@ -355,6 +411,7 @@ static void MouseKeySet()
 //        chassis_cmd_send.chassis_speed_buff = 100;
 //        break;
 //    }
+
     switch (rc_data[TEMP].key[KEY_PRESS].shift) // 待添加 按shift允许超功率 消耗缓冲能量
     {
     case 1:
@@ -365,6 +422,10 @@ static void MouseKeySet()
 
         break;
     }
+
+    pitch_limit();
+    death_check();
+
 }
 
 /**

application/gimbal/gimbal.c

@@ -27,17 +27,17 @@ void GimbalInit()
         },
         .controller_param_init_config = {
             .angle_PID = {
-                .Kp = 200, // 8
+                .Kp = 4, // 8
                 .Ki = 0,
-                .Kd = 20,
+                .Kd = 0.3,
                 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                 .IntegralLimit = 100,
 
                 .MaxOut = 500,
             },
             .speed_PID = {
-                .Kp = 70,  // 70
+                .Kp = 5000,  // 70
-                .Ki = 0, // 200
+                .Ki = 1000, // 200
                 .Kd = 0,//10
                 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                 .IntegralLimit = 3000,
@@ -52,7 +52,7 @@ void GimbalInit()
             .speed_feedback_source = OTHER_FEED,
             .outer_loop_type = ANGLE_LOOP,
             .close_loop_type =  ANGLE_LOOP |SPEED_LOOP,
-            .motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
+            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
         },
         .motor_type = GM6020
     };
@@ -64,20 +64,20 @@ void GimbalInit()
         },
         .controller_param_init_config = {
             .angle_PID = {
-                .Kp = 20, // 65
+                .Kp = 0.3f,//20, // 65
                 .Ki = 0,
-                .Kd = 1,
+                .Kd = 0,//1,
                 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                 .IntegralLimit = 100,//100
                 .MaxOut = 500,//500
             },
             .speed_PID = {
-                .Kp = 30,  // 50
+                .Kp = 2500,//11000,  // 50
-                .Ki = 0, // 350
+                .Ki = 1000, // 350
-                .Kd = 2,   // 0
+                .Kd = 0,//15000,   // 0
                 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
-                .IntegralLimit = 150,//2500
+                .IntegralLimit = 2500,
-                .MaxOut = 2000,//20000
+                .MaxOut = 20000,
             },
             .other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
             // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明

application/shoot/shoot.c

@@ -193,9 +193,9 @@ void ShootTask()
     // 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启)
     if (shoot_cmd_recv.friction_mode == FRICTION_ON)
     {
-            DJIMotorSetRef(friction_l, 36000);
+            DJIMotorSetRef(friction_l, 35000);
 //            DJIMotorSetRef(friction_r, 39000);
-            DJIMotorSetRef(friction_z, 36000);//39000/6 = 6500
+            DJIMotorSetRef(friction_z, 35000);//39000/6 = 6500
     }
     else // 关闭摩擦轮
     {

modules/auto_aim/auto_aim.c

@@ -30,14 +30,6 @@ int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_c
             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);
@@ -395,7 +387,7 @@ int aim_center_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_
   * @param[in]      v_x0:弹速水平分量
   * @retval         返回空
   */
-const float k1 = 0.0761; //标准大气压25度下空气阻力系数
+const float k1 = 0.015; //标准大气压25度下空气阻力系数
 float get_fly_time(float x, float vx, float v_x0) {
     float t = 0;
     float f_ti = 0, df_ti = 0;