超电功率控制，云台pid，UI更新

application/chassis/chassis.c

@@ -52,8 +52,7 @@ const static float CHASSIS_ACCEL_Y_NUM=0.1f;
 
 // 超级电容
 SuperCapInstance *cap;    // 超级电容全局
-float power_set;          // 超级电容功率设置
+static uint16_t last_chassis_power_limit=0;//超级电容更新
-
 static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
 
 /* 用于自旋变速策略的时间变量 */
@@ -92,7 +91,7 @@ void ChassisInit() {
                     .speed_feedback_source = MOTOR_FEED,
                     .outer_loop_type = SPEED_LOOP,
                     .close_loop_type = SPEED_LOOP, // CURRENT_LOOP,
-                    .power_limit_flag = POWER_LIMIT_ON,
+                    .power_limit_flag = NO_POWER_LIMIT,
             },
             .motor_type = M3508,
     };
@@ -102,12 +101,12 @@ void ChassisInit() {
     chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
     chassis_motor_config.controller_param_init_config.speed_PID.Kp = 5.0f;
     chassis_motor_config.controller_param_init_config.speed_PID.Ki = 0.01f;
-    chassis_motor_config.controller_param_init_config.speed_PID.Kd = 0.01f;
+    chassis_motor_config.controller_param_init_config.speed_PID.Kd = 0.001f;
     motor_rf = DJIMotorInit(&chassis_motor_config);
 
     chassis_motor_config.can_init_config.tx_id = 2;
     chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
-    chassis_motor_config.controller_param_init_config.speed_PID.Kp = 5.1f;
+    chassis_motor_config.controller_param_init_config.speed_PID.Kp = 5.0f;
     chassis_motor_config.controller_param_init_config.speed_PID.Ki = 0.01f;
     chassis_motor_config.controller_param_init_config.speed_PID.Kd = 0.001f;
     motor_lf = DJIMotorInit(&chassis_motor_config);
@@ -116,14 +115,14 @@ void ChassisInit() {
     chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
     chassis_motor_config.controller_param_init_config.speed_PID.Kp = 5.0f;
     chassis_motor_config.controller_param_init_config.speed_PID.Ki = 0.01f;
-    chassis_motor_config.controller_param_init_config.speed_PID.Kd = 0.01f;
+    chassis_motor_config.controller_param_init_config.speed_PID.Kd = 0.001f;
     motor_lb = DJIMotorInit(&chassis_motor_config);
 
     chassis_motor_config.can_init_config.tx_id = 4;
     chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
-    chassis_motor_config.controller_param_init_config.speed_PID.Kp = 5.1f;
+    chassis_motor_config.controller_param_init_config.speed_PID.Kp = 5.0f;
     chassis_motor_config.controller_param_init_config.speed_PID.Ki = 0.01f;
-    chassis_motor_config.controller_param_init_config.speed_PID.Kd = 0.01f;
+    chassis_motor_config.controller_param_init_config.speed_PID.Kd = 0.001f;
     motor_rb = DJIMotorInit(&chassis_motor_config);
 
     //超级电容
@@ -134,8 +133,6 @@ void ChassisInit() {
                     .rx_id = 0x211,
             }};
     cap = SuperCapInit(&cap_conf); // 超级电容初始化
-    power_set = 70;
-    SuperCapSetPower(cap,power_set);  // 超级电容限制功率
 
     //用一阶滤波代替斜波函数生成                                     //增大更能刹住
     first_order_filter_init(&vx_filter, 0.007f, &CHASSIS_ACCEL_X_NUM);
@@ -217,9 +214,10 @@ static void LimitChassisOutput()
     else if(chassis_cmd_recv.buffer_energy<10&&chassis_cmd_recv.buffer_energy>=0)	Plimit=0.05f;
     else if(chassis_cmd_recv.buffer_energy==60)					                    Plimit=1.0f;
 
-    float K = ((float)(chassis_cmd_recv.chassis_power_limit - 3) * (Plimit + 0.50f) ) / P_cmd;
+    float K = ((float)(chassis_cmd_recv.chassis_power_limit - 3) * (Plimit + 0.50f) + (float)(chassis_cmd_recv.buffer_supercap)) / P_cmd;
-    //float K = (float)(chassis_cmd_recv.chassis_power_limit - 5) / P_cmd;
 
+    if(chassis_cmd_recv.buffer_energy<5)//当缓冲功率过小时，限制功率给小；
+        K = (float)(chassis_cmd_recv.chassis_power_limit - 3) / P_cmd;
     motor_rf->motor_controller.motor_power_scale = K;
     motor_rb->motor_controller.motor_power_scale = K;
     motor_lf->motor_controller.motor_power_scale = K;
@@ -233,7 +231,19 @@ static void LimitChassisOutput()
     }
 
 }
-
+/**
+ * @brief 每次随等级更新超电的设定功率
+ *
+ *
+ */
+static void SuperCapSetUpdate()
+{
+    if(last_chassis_power_limit != chassis_cmd_recv.chassis_power_limit)
+    {
+        SuperCapSetPower(cap,chassis_cmd_recv.chassis_power_limit);  // 超级电容限制功率
+        last_chassis_power_limit = chassis_cmd_recv.chassis_power_limit;
+    }
+}
 /**
  * @brief 根据每个轮子的速度反馈,计算底盘的实际运动速度,逆运动解算
  *        对于双板的情况,考虑增加来自底盘板IMU的数据
@@ -326,7 +336,8 @@ void ChassisTask() {
     //chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit;
 
     chassis_feedback_data.cap_power = cap->cap_msg.cap_vol;
-
+    //每次随等级更新超电的设定功率
+    SuperCapSetUpdate();
     // 推送反馈消息
 #ifdef ONE_BOARD
     PubPushMessage(chassis_pub, (void *) &chassis_feedback_data);

application/cmd/robot_cmd.c

@@ -62,7 +62,7 @@ static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息
 static Robot_Status_e robot_state; // 机器人整体工作状态
 
 static referee_info_t *referee_data; // 用于获取裁判系统的数据
-static uint8_t load_flag = 0;      //拨弹模式选择标志位
+static uint8_t loader_flag = 0;      //拨弹模式选择标志位
 
 void RobotCMDInit() {
     rc_data = RemoteControlInit(&huart3);   // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
@@ -145,7 +145,7 @@ static void update_ui_data() {
 }
 
 static void auto_aim_mode() {
-    trajectory_cal.v0 = 25; //弹速30
+    trajectory_cal.v0 = 20; //弹速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;
@@ -202,15 +202,14 @@ static void RemoteControlSet() {
 
     } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],,底盘跟随云台
     {
-
         chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
         gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
+        shoot_cmd_send.lid_mode = LID_CLOSE;
+    }
+    else if (switch_is_up(rc_data[TEMP].rc.switch_right))// 右侧开关状态[上]
+    {
+        shoot_cmd_send.lid_mode = LID_OPEN;
     }
-//    else if (switch_is_up(rc_data[TEMP].rc.switch_right))// 右侧开关状态[上],小陀螺
-//    {
-//        chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
-//        gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
-//    }
 
     // 云台参数,确定云台控制数据
     if (switch_is_mid(rc_data[TEMP].rc.switch_left) ||
@@ -290,14 +289,17 @@ static void MouseKeySet() {
     gimbal_cmd_send.yaw -= (float) rc_data[TEMP].mouse.x / 660 * 4; // 系数待测
     gimbal_cmd_send.pitch += (float) rc_data[TEMP].mouse.y / 660 * 6;
 
+    if (gimbal_cmd_send.pitch >= PITCH_MAX_ANGLE) gimbal_cmd_send.pitch = PITCH_MAX_ANGLE;
+    if (gimbal_cmd_send.pitch <= PITCH_MIN_ANGLE) gimbal_cmd_send.pitch = PITCH_MIN_ANGLE;
+
     aim_select.suggest_fire = 0;
     if (rc_data[TEMP].mouse.press_l && (!rc_data[TEMP].mouse.press_r)) // 左键发射模式
     {
         if (shoot_cmd_send.friction_mode == FRICTION_ON) {
             shoot_cmd_send.shoot_mode = SHOOT_ON;
-            if(load_flag == 0){
+            if(loader_flag == 0){
                 shoot_cmd_send.loader_mode = LOAD_BURSTFIRE;
-            }else if(load_flag == 1){
+            }else if(loader_flag == 1){
                 shoot_cmd_send.loader_mode = LOAD_3_BULLET;
             }else
                 shoot_cmd_send.loader_mode = LOAD_1_BULLET;
@@ -305,7 +307,6 @@ static void MouseKeySet() {
     } else if ((!rc_data[TEMP].mouse.press_l) && (!rc_data[TEMP].mouse.press_r))
     {
         shoot_cmd_send.loader_mode = LOAD_STOP;
-
     }
     if (rc_data[TEMP].mouse.press_r) // 右键自瞄模式
     {
@@ -355,13 +356,13 @@ static void MouseKeySet() {
             shoot_cmd_send.friction_mode = FRICTION_ON;
             break;
     }
-    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q键开关弹舱盖
+    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_B] % 2) // Q键开关弹舱盖
     {
         case 0:
-            shoot_cmd_send.lid_mode = LID_OPEN;
+            shoot_cmd_send.lid_mode = LID_CLOSE;
             break;
         default:
-            shoot_cmd_send.lid_mode = LID_CLOSE;
+            shoot_cmd_send.lid_mode = LID_OPEN;
             break;
     }
     switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺
@@ -378,13 +379,13 @@ static void MouseKeySet() {
     switch (rc_data[TEMP].key_count[KEY_PRESS][Key_G] % 3) // G键切换发射模式
     {
         case 0:
-            load_flag = 0;
+            loader_flag = 0;
             break;
         case 1:
-            load_flag = 1;
+            loader_flag = 1;
             break;
         default:
-            load_flag = 3;
+            loader_flag = 3;
             break;
     }
     switch (rc_data[TEMP].key[KEY_PRESS].shift) //按shift允许消耗超级电容能量
@@ -417,6 +418,9 @@ static void VTMouseKeySet()
     gimbal_cmd_send.yaw -= (float) vt_data[TEMP].mouse.x / 660 * 4; // 系数待测
     gimbal_cmd_send.pitch += (float) vt_data[TEMP].mouse.y / 660 * 6;
 
+    if (gimbal_cmd_send.pitch >= PITCH_MAX_ANGLE) gimbal_cmd_send.pitch = PITCH_MAX_ANGLE;
+    if (gimbal_cmd_send.pitch <= PITCH_MIN_ANGLE) gimbal_cmd_send.pitch = PITCH_MIN_ANGLE;
+
     aim_select.suggest_fire = 0;
     if (vt_data[TEMP].mouse.press_l && (!vt_data[TEMP].mouse.press_r)) // 左键发射模式
     {
@@ -477,13 +481,13 @@ static void VTMouseKeySet()
             shoot_cmd_send.friction_mode = FRICTION_ON;
             break;
     }
-    switch (vt_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q键开关弹舱盖
+    switch (vt_data[TEMP].key_count[KEY_PRESS][Key_B] % 2) // Q键开关弹舱盖
     {
         case 0:
-            shoot_cmd_send.lid_mode = LID_OPEN;
+            shoot_cmd_send.lid_mode = LID_CLOSE;
             break;
         default:
-            shoot_cmd_send.lid_mode = LID_CLOSE;
+            shoot_cmd_send.lid_mode = LID_OPEN;
             break;
     }
     switch (vt_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺
@@ -500,22 +504,22 @@ static void VTMouseKeySet()
     switch (rc_data[TEMP].key_count[KEY_PRESS][Key_G] % 3) // G键切换发射模式
     {
         case 0:
-            load_flag = 0;
+            loader_flag = 0;
             break;
         case 1:
-            load_flag = 1;
+            loader_flag = 1;
             break;
         default:
-            load_flag = 3;
+            loader_flag = 3;
             break;
     }
     switch (vt_data[TEMP].key[KEY_PRESS].shift) //按shift允许消耗超级电容能量
     {
         case 0:
-            SuperCapSetPower(cap,40);
+            chassis_cmd_send.buffer_supercap = 0;
             break;
         default:
-            SuperCapSetPower(cap,70);
+            chassis_cmd_send.buffer_supercap = 195;
             break;
     }
 

application/gimbal/gimbal.c

@@ -50,21 +50,21 @@ void GimbalInit() {
             },
             .controller_param_init_config = {
                     .angle_PID = {
-                            .Kp = 1.2f,//4
+                            .Kp = 0.8f,//1.2
-                            .Ki = 0.0f,
+                            .Ki = 0.0f,//0
-                            .Kd = 0.05f,
+                            .Kd = 0.04f,//0.05
                             .DeadBand = 0.0f,
                             .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                             .IntegralLimit = 100,
                             .MaxOut = 500,
                     },
                     .speed_PID = {
-                            .Kp =  4000,  // 2480
+                            .Kp = 5000,  //4000
-                            .Ki = 100.0f, // 200
+                            .Ki = 100.0f, //100
-                            .Kd = 0.0f,
+                            .Kd = 0.03f, //0
                             .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                             .IntegralLimit = 10000,
-                            .MaxOut = 20000,
+                            .MaxOut = 15000,
                     },
                     .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
                     // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
@@ -88,9 +88,9 @@ void GimbalInit() {
             },
             .controller_param_init_config = {
                     .angle_PID = {
-                            .Kp = 0.9f,
+                            .Kp = 1.0f,
                             .Ki = 0.0f,
-                            .Kd = 0.0f,
+                            .Kd = 0.02f,
                             .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                             .IntegralLimit = 100,
                             .MaxOut = 500,
@@ -101,7 +101,7 @@ void GimbalInit() {
                             .Kd = 0.0f,   // 0
                             .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                             .IntegralLimit = 10000,
-                            .MaxOut = 20000,
+                            .MaxOut = 15000,
                     },
                     .other_angle_feedback_ptr = &gimba_IMU_data->Roll,
                     // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明

application/robot_def.h

@@ -29,8 +29,8 @@
 #define YAW_CHASSIS_ALIGN_ECD 1443 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
 #define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
 #define PITCH_HORIZON_ECD 4422    // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
-#define PITCH_MAX_ANGLE 18           // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
+#define PITCH_MAX_ANGLE 25           // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
-#define PITCH_MIN_ANGLE -30          // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
+#define PITCH_MIN_ANGLE -24          // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
 
 #define PITCH_MAX_RELATIVE_ANGLE 123        // 云台相对底盘最大角度
 #define PITCH_MIN_RELATIVE_ANGLE 80          // 云台相对底盘最小角度
@@ -152,6 +152,7 @@ typedef struct
 
     uint16_t chassis_power_limit;
     uint16_t buffer_energy;
+    uint16_t buffer_supercap;
     // UI部分
     //  ...
 

application/shoot/shoot.c

@@ -268,11 +268,11 @@ void ShootTask()
     // 开关弹舱盖
     if (shoot_cmd_recv.lid_mode == LID_CLOSE)
     {
-        Servo_Motor_FreeAngle_Set(lid,95);
+        Servo_Motor_FreeAngle_Set(lid,107);
     }
     else if (shoot_cmd_recv.lid_mode == LID_OPEN)
     {
-        Servo_Motor_FreeAngle_Set(lid,20);
+        Servo_Motor_FreeAngle_Set(lid,10);
     }
     //卡弹检测
     stalled_detect();

modules/referee/referee_task.c

@@ -22,6 +22,7 @@ static Referee_Interactive_info_t *Interactive_data; // UI绘制需要的机器
 static referee_info_t *referee_recv_info;            // 接收到的裁判系统数据
 Referee_Interactive_info_t ui_data;
 uint8_t UI_Seq;                                      // 包序号，供整个referee文件使用
+extern uint8_t loader_flag = 0;      //拨弹模式选择标志位
 // @todo 不应该使用全局变量
 
 /**
@@ -63,6 +64,7 @@ static Graph_Data_t UI_Energy[4];      // 电容能量条
 static String_Data_t UI_State_sta[7];  // 机器人状态,静态只需画一次
 static String_Data_t UI_State_dyn[7];  // 机器人状态,动态先add才能change
 static uint32_t shoot_line_location[10] = {540, 960, 490, 515, 565};
+static String_Data_t UI_prompt_sta[3];  // 操作手提示
 
 void MyUIInit()
 {
@@ -83,36 +85,43 @@ void MyUIInit()
     UIGraphRefresh(&referee_recv_info->referee_id, 5, UI_shoot_line[0], UI_shoot_line[1], UI_shoot_line[2], UI_shoot_line[3],UI_shoot_yuan[0]);
 
     // 绘制车辆状态标志指示，静态
-    UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 150, 750, "chassis:");
+    UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Main, 20, 2, 10, 750, "chassis:");
     UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[0]);
-    UICharDraw(&UI_State_sta[1], "ss1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 150, 700, "gimbal:");
+//    UICharDraw(&UI_State_sta[1], "ss1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 150, 700, "gimbal:");
-    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[1]);
+//    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[1]);
-    UICharDraw(&UI_State_sta[2], "ss2", UI_Graph_ADD, 8, UI_Color_Orange, 15, 2, 150, 650, "shoot:");
+//    UICharDraw(&UI_State_sta[2], "ss2", UI_Graph_ADD, 8, UI_Color_Orange, 15, 2, 150, 650, "shoot:");
-    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[2]);
+//    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[2]);
-    UICharDraw(&UI_State_sta[3], "ss3", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 150, 600, "F.frict:");
+    UICharDraw(&UI_State_sta[3], "ss3", UI_Graph_ADD, 8, UI_Color_Pink, 20, 2, 10, 700, "F.frict:");
     UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[3]);
-    UICharDraw(&UI_State_sta[4], "ss4", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 150, 550, "Q.lid:");
+    UICharDraw(&UI_State_sta[4], "ss4", UI_Graph_ADD, 8, UI_Color_Pink, 20, 2, 10, 650, "Q.lid:");
     UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[4]);
-    UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 150, 800, "load:");
+//    UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 150, 800, "load:");
-    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[5]);
+//    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[5]);
+
+    // 操作手提示
+    UICharDraw(&UI_prompt_sta[0], "ss8", UI_Graph_ADD,8, UI_Color_Pink, 30, 4, 1650, 700, "leng jing");
+    UICharRefresh(&referee_recv_info->referee_id, UI_prompt_sta[0]);
+    UICharDraw(&UI_prompt_sta[1], "ss9", UI_Graph_ADD, 8,UI_Color_Pink, 30, 4, 1650, 800, "zhi hui");
+    UICharRefresh(&referee_recv_info->referee_id, UI_prompt_sta[1]);
+
     // 底盘功率显示，静态
     UICharDraw(&UI_State_sta[6], "ss6", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 620, 230, "Power_MAX:");
     UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[6]);
 
     // 绘制车辆状态标志，动态
     // 由于初始化时xxx_last_mode默认为0，所以此处对应UI也应该设为0时对应的UI，防止模式不变的情况下无法置位flag，导致UI无法刷新
-    UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce");
+    UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_ADD, 8, UI_Color_Main, 20, 2, 170, 750, "zeroforce");
     UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]);
-    UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce");
+//    UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce");
-    UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
+//    UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
-    UICharDraw(&UI_State_dyn[2], "sd2", UI_Graph_ADD, 8, UI_Color_Orange, 15, 2, 270, 650, "off");
+//    UICharDraw(&UI_State_dyn[2], "sd2", UI_Graph_ADD, 8, UI_Color_Orange, 15, 2, 270, 650, "off");
-    UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[2]);
+//    UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[2]);
-    UICharDraw(&UI_State_dyn[3], "sd3", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 270, 600, "off");
+    UICharDraw(&UI_State_dyn[3], "sd3", UI_Graph_ADD, 8, UI_Color_Pink, 20, 2, 170, 700, "off");
     UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[3]);
-    UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 270, 550, "open ");
+    UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_ADD, 8, UI_Color_Pink, 20, 2, 170, 650, "on ");
     UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]);
-    UICharDraw(&UI_State_dyn[5], "sd5", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 270, 800, "999");
+//    UICharDraw(&UI_State_dyn[5], "sd5", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 270, 800, "999");
-    UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[5]);
+//    UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[5]);
 
     // 能量条框
     UIRectangleDraw(&UI_Energy[0], "ss6", UI_Graph_ADD, 7, UI_Color_Green, 2, 720, 140, 1220, 180);
@@ -139,91 +148,91 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i
         switch (_Interactive_data->chassis_mode)
         {
         case CHASSIS_ZERO_FORCE:
-            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce");
+            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 20, 2, 170, 750, "zeroforce");
             break;
         case CHASSIS_ROTATE:
-            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "rotate   ");
+            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 20, 2, 170, 750, "rotate   ");
             // 此处注意字数对齐问题，字数相同才能覆盖掉
             break;
         case CHASSIS_NO_FOLLOW:
-            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "nofollow ");
+            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 20, 2, 170, 750, "nofollow ");
             break;
         case CHASSIS_FOLLOW_GIMBAL_YAW:
-            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "follow   ");
+            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 20, 2, 170, 750, "follow   ");
             break;
         }
         UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]);
         _Interactive_data->Referee_Interactive_Flag.chassis_flag = 0;
     }
     // gimbal
-    if (_Interactive_data->Referee_Interactive_Flag.gimbal_flag == 1)
+//    if (_Interactive_data->Referee_Interactive_Flag.gimbal_flag == 1)
-    {
+//    {
-        switch (_Interactive_data->gimbal_mode)
+//        switch (_Interactive_data->gimbal_mode)
-        {
+//        {
-        case GIMBAL_ZERO_FORCE:
+//        case GIMBAL_ZERO_FORCE:
-        {
+//        {
-            UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce");
+//            UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce");
-            break;
+//            break;
-        }
+//        }
-        case GIMBAL_FREE_MODE:
+//        case GIMBAL_FREE_MODE:
-        {
+//        {
-            UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "free     ");
+//            UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "free     ");
-            break;
+//            break;
-        }
+//        }
-        case GIMBAL_GYRO_MODE:
+//        case GIMBAL_GYRO_MODE:
-        {
+//        {
-            UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "gyro     ");
+//            UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "gyro     ");
-            break;
+//            break;
-        }
+//        }
-        }
+//        }
-        UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
+//        UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
-        _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 0;
+//        _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 0;
-    }
+//    }
     // shoot
-    if (_Interactive_data->Referee_Interactive_Flag.shoot_flag == 1)
+//    if (_Interactive_data->Referee_Interactive_Flag.shoot_flag == 1)
-    {
+//    {
-        UICharDraw(&UI_State_dyn[2], "sd2", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 650, _Interactive_data->shoot_mode == SHOOT_ON ? "on " : "off");
+//        UICharDraw(&UI_State_dyn[2], "sd2", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 650, _Interactive_data->shoot_mode == SHOOT_ON ? "on " : "off");
-        UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[2]);
+//        UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[2]);
-        _Interactive_data->Referee_Interactive_Flag.shoot_flag = 0;
+//        _Interactive_data->Referee_Interactive_Flag.shoot_flag = 0;
-    }
+//    }
     // friction
     if (_Interactive_data->Referee_Interactive_Flag.friction_flag == 1)
     {
-        UICharDraw(&UI_State_dyn[3], "sd3", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 600, _Interactive_data->friction_mode == FRICTION_ON ? "on " : "off");
+        UICharDraw(&UI_State_dyn[3], "sd3", UI_Graph_Change, 8, UI_Color_Pink, 20, 2, 170, 700, _Interactive_data->friction_mode == FRICTION_ON ? "on " : "off");
         UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[3]);
         _Interactive_data->Referee_Interactive_Flag.friction_flag = 0;
     }
     // lid
     if (_Interactive_data->Referee_Interactive_Flag.lid_flag == 1)
     {
-        UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 550, _Interactive_data->lid_mode == LID_OPEN ? "open " : "close");
+        UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_Change, 8, UI_Color_Pink, 20, 2, 170, 650, _Interactive_data->lid_mode == LID_OPEN ? "on " : "off");
         UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]);
         _Interactive_data->Referee_Interactive_Flag.lid_flag = 0;
     }
-    // load_mode
+    // loader_mode
-    if (_Interactive_data->Referee_Interactive_Flag.load_flag == 1)
+//    if (_Interactive_data->Referee_Interactive_Flag.load_flag == 1)
-    {
+//    {
-        switch (_Interactive_data->loader_mode)
+//        switch (loader_flag)
-        {
+//        {
-            case LOAD_1_BULLET:
+//            case 2:
-            {
+//            {
-                UICharDraw(&UI_State_dyn[5], "sd5", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 800, " 1 ");
+//                UICharDraw(&UI_State_dyn[5], "sd5", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 800, " 1 ");
-                break;
+//                break;
-            }
+//            }
-            case LOAD_3_BULLET:
+//            case 1:
-            {
+//            {
-                UICharDraw(&UI_State_dyn[5], "sd5", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 800, " 3 ");
+//                UICharDraw(&UI_State_dyn[5], "sd5", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 800, " 3 ");
-                break;
+//                break;
-            }
+//            }
-            case LOAD_BURSTFIRE:
+//            case 0:
-            {
+//            {
-                UICharDraw(&UI_State_dyn[5], "sd5", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 800, "999");
+//                UICharDraw(&UI_State_dyn[5], "sd5", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 800, "999");
-                break;
+//                break;
-            }
+//            }
-        }
+//        }
-        UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[5]);
+//        UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[5]);
-        _Interactive_data->Referee_Interactive_Flag.load_flag = 0;
+//        _Interactive_data->Referee_Interactive_Flag.load_flag = 0;
-    }
+//    }
     // power
     if (_Interactive_data->Referee_Interactive_Flag.Power_flag == 1)
     {
@@ -260,17 +269,17 @@ static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data)
         _Interactive_data->chassis_last_mode = _Interactive_data->chassis_mode;
     }
 
-    if (_Interactive_data->gimbal_mode != _Interactive_data->gimbal_last_mode)
+//    if (_Interactive_data->gimbal_mode != _Interactive_data->gimbal_last_mode)
-    {
+//    {
-        _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 1;
+//        _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 1;
-        _Interactive_data->gimbal_last_mode = _Interactive_data->gimbal_mode;
+//        _Interactive_data->gimbal_last_mode = _Interactive_data->gimbal_mode;
-    }
+//    }
 
-    if (_Interactive_data->shoot_mode != _Interactive_data->shoot_last_mode)
+//    if (_Interactive_data->shoot_mode != _Interactive_data->shoot_last_mode)
-    {
+//    {
-        _Interactive_data->Referee_Interactive_Flag.shoot_flag = 1;
+//        _Interactive_data->Referee_Interactive_Flag.shoot_flag = 1;
-        _Interactive_data->shoot_last_mode = _Interactive_data->shoot_mode;
+//        _Interactive_data->shoot_last_mode = _Interactive_data->shoot_mode;
-    }
+//    }
 
     if (_Interactive_data->friction_mode != _Interactive_data->friction_last_mode)
     {
@@ -302,9 +311,9 @@ static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data)
         _Interactive_data->aim_last_fire = _Interactive_data->aim_fire;
     }
 
-    if (_Interactive_data->loader_mode != _Interactive_data->loader_last_mode)
+//    if (_Interactive_data->loader_mode != _Interactive_data->loader_last_mode)
-    {
+//    {
-        _Interactive_data->Referee_Interactive_Flag.load_flag = 1;
+//        _Interactive_data->Referee_Interactive_Flag.load_flag = 1;
-        _Interactive_data->loader_last_mode = _Interactive_data->loader_mode;
+//        _Interactive_data->loader_last_mode = _Interactive_data->loader_mode;
-    }
+//    }
 }

xs.jdebug.user

@@ -1,26 +1,24 @@
 
 
-GraphedExpression="(((motor_lf)->motor_controller).speed_PID).Measure", Color=#e56a6f
+OpenDocument="gimbal.c", FilePath="D:/zhandui/cqdm/bubing_9/application/gimbal/gimbal.c", Line=0
-GraphedExpression="(((motor_lf)->motor_controller).speed_PID).Ref", Color=#35792b
+OpenDocument="main.c", FilePath="D:/zhandui/cqdm/bubing_9/Src/main.c", Line=51
-OpenDocument="robot_cmd.c", FilePath="D:/zhandui/cqdm/bubing_9/application/cmd/robot_cmd.c", Line=387
 OpenDocument="tasks.c", FilePath="D:/zhandui/cqdm/bubing_9/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3418
-OpenDocument="main.c", FilePath="D:/zhandui/cqdm/bubing_9/Src/main.c", Line=65
+OpenDocument="robot_cmd.c", FilePath="D:/zhandui/cqdm/bubing_9/application/cmd/robot_cmd.c", Line=387
 OpenDocument="chassis.c", FilePath="D:/zhandui/cqdm/bubing_9/application/chassis/chassis.c", Line=87
 OpenDocument="shoot.c", FilePath="D:/zhandui/cqdm/bubing_9/application/shoot/shoot.c", Line=191
 OpenDocument="super_cap.c", FilePath="D:/zhandui/cqdm/bubing_9/modules/super_cap/super_cap.c", Line=0
 OpenToolbar="Debug", Floating=0, x=0, y=0
-OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=735, h=459, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=735, h=479, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
-OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=1, w=1150, h=319, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=621, h=384, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x30A3D70C
-OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=552, h=384, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x30A3D70C
 OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=735, h=577, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
-OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=473, h=384, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=531, h=384, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
-OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=1294, h=384, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
+OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=1456, h=384, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
-OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=1150, h=717, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;502", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="923;-69", CodeGraphLegendShown=0, CodeGraphLegendPosition="939;0"
+OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=1197, h=1057, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;841", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="923;-69", CodeGraphLegendShown=0, CodeGraphLegendPosition="939;0"
-OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=238, h=384, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=269, h=384, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
 TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[263;100;100;100;950]
-TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" (((motor_lf)->motor_controller).speed_PID).Measure";" (((motor_lf)->motor_controller).speed_PID).Ref"], ColWidths=[100;100;100;100]
+TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time"], ColWidths=[100;100]
-TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[438;100;114;124;114;124;110;126;126]
+TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[438;100;100;100;100;100;110;126;282]
 TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
 TableHeader="Task List", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Name";"Run Count";"Priority";"Status";"Timeout";"Stack Info";"ID";"Mutex Count";"Notified Value";"Notify State"], ColWidths=[110;110;110;110;110;110;110;110;110;110]
 TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[238;149;104;218]
@@ -39,4 +37,6 @@ WatchedExpression="loader", RefreshRate=2, Window=Watched Data 1
 WatchedExpression="shoot.c::shoot_cmd_recv", RefreshRate=2, Window=Watched Data 1
 WatchedExpression="loader", RefreshRate=2, Window=Watched Data 1
 WatchedExpression="cap", RefreshRate=2, Window=Watched Data 1
-WatchedExpression="shoot_cmd_recv", RefreshRate=5, Window=Watched Data 1
+WatchedExpression="shoot_cmd_recv", RefreshRate=5, Window=Watched Data 1
+WatchedExpression="gimba_IMU_data", RefreshRate=2, Window=Watched Data 1
+WatchedExpression="vt_data", RefreshRate=2, Window=Watched Data 1