更新UI

功率控制加超电，参数可以更激进一点
更新裁判系统串口命令码数据段长度，解决缓冲功率接收错误的问题
2024-05-17 06:23:21 +08:00 · 2024-05-16 22:15:26 +08:00 · 2024-05-16 17:55:25 +08:00 · 2024-05-16 15:30:08 +08:00 · 2024-05-16 01:53:47 +08:00 · 2024-05-14 02:34:46 +08:00
43 changed files with 2333 additions and 400 deletions
--- a/.idea/runConfigurations/OCD_basic_framework.xml
+++ b/.idea/runConfigurations/OCD_basic_framework.xml
@ -1,6 +1,6 @@
 <component name="ProjectRunConfigurationManager">
-  <configuration default="false" name="OCD basic_framework" type="com.jetbrains.cidr.embedded.openocd.conf.type" factoryName="com.jetbrains.cidr.embedded.openocd.conf.factory" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="basic_framework" TARGET_NAME="basic_framework.elf" CONFIG_NAME="Debug" version="1" RUN_TARGET_PROJECT_NAME="basic_framework" RUN_TARGET_NAME="basic_framework.elf">
-    <openocd version="1" gdb-port="3333" telnet-port="4444" board-config="D:\CLion\Project\basic_framework\daplink.cfg" reset-type="INIT" download-type="UPDATED_ONLY">
+  <configuration default="false" name="OCD basic_framework" type="com.jetbrains.cidr.embedded.openocd.conf.type" factoryName="com.jetbrains.cidr.embedded.openocd.conf.factory" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" EMULATE_TERMINAL="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="basic_framework" TARGET_NAME="basic_framework.elf" CONFIG_NAME="Debug" version="1" RUN_TARGET_PROJECT_NAME="basic_framework" RUN_TARGET_NAME="basic_framework.elf">
+    <openocd version="1" gdb-port="3333" telnet-port="4444" board-config="C:\Users\24871\Desktop\RM\RM2024\basic_framework\daplink.cfg" reset-type="INIT" download-type="UPDATED_ONLY">
      <debugger kind="GDB" isBundled="true" />
    </openocd>
    <method v="2">
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -52,9 +52,9 @@ endif ()
 include_directories(Inc Drivers/STM32F4xx_HAL_Driver/Inc Drivers/STM32F4xx_HAL_Driver/Inc/Legacy Middlewares/Third_Party/FreeRTOS/Source/include Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F Middlewares/ST/STM32_USB_Device_Library/Core/Inc Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc Drivers/CMSIS/Device/ST/STM32F4xx/Include Drivers/CMSIS/Include Middlewares/ST/ARM/DSP/Inc
        bsp bsp/adc bsp/can bsp/dwt bsp/flash bsp/gpio bsp/iic bsp/log bsp/pwm bsp/spi bsp/usart bsp/usb
        modules modules/alarm modules/algorithm modules/BMI088 modules/can_comm modules/daemon modules/encoder modules/imu modules/ist8310
-        modules/led modules/master_machine modules/message_center modules/motor modules/oled modules/referee modules/remote modules/standard_cmd
-        modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa
-        modules/motor/DJImotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor
+        modules/led modules/master_machine modules/message_center modules/motor modules/oled modules/power_meter modules/referee modules/remote modules/standard_cmd
+        modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa modules/auto_aim
+        modules/motor/DJImotor modules/motor/DMmotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor
        application application/chassis application/cmd application/gimbal application/shoot
        Middlewares/Third_Party/SEGGER/RTT Middlewares/Third_Party/SEGGER/Config)

@ -85,4 +85,4 @@ add_custom_command(TARGET ${PROJECT_NAME}.elf POST_BUILD
        COMMAND ${CMAKE_OBJCOPY} -Oihex $<TARGET_FILE:${PROJECT_NAME}.elf> ${HEX_FILE}
        COMMAND ${CMAKE_OBJCOPY} -Obinary $<TARGET_FILE:${PROJECT_NAME}.elf> ${BIN_FILE}
        COMMENT "Building ${HEX_FILE}
-Building ${BIN_FILE}")
+Building ${BIN_FILE}")
--- a/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.c
+++ b/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.c
@ -26,7 +26,7 @@
 *  
 *----------------------------------------------------------------------------
 *
- * Portions Copyright © 2016 STMicroelectronics International N.V. All rights reserved.
+ * Portions Copyright <EFBFBD> 2016 STMicroelectronics International N.V. All rights reserved.
 * Portions Copyright (c) 2013 ARM LIMITED
 * All rights reserved.
 * Redistribution and use in source and binary forms, with or without
--- a/Src/gpio.c
+++ b/Src/gpio.c
@ -103,6 +103,13 @@ void MX_GPIO_Init(void)
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
  HAL_GPIO_Init(CS1_GYRO_GPIO_Port, &GPIO_InitStruct);

+    /*Configure GPIO pin : PtPin */
+    GPIO_InitStruct.Pin = GPIO_PIN_8;
+    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+    GPIO_InitStruct.Pull = GPIO_PULLUP;
+    HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
+    HAL_GPIO_WritePin(GPIOC,GPIO_PIN_8,GPIO_PIN_SET);
+
  /* EXTI interrupt init*/
  HAL_NVIC_SetPriority(EXTI3_IRQn, 5, 0);
  HAL_NVIC_EnableIRQ(EXTI3_IRQn);
--- a/Src/usart.c
+++ b/Src/usart.c
@ -46,7 +46,7 @@ void MX_USART1_UART_Init(void)

  /* USER CODE END USART1_Init 1 */
  huart1.Instance = USART1;
-  huart1.Init.BaudRate = 921600;
+  huart1.Init.BaudRate = 115200;
  huart1.Init.WordLength = UART_WORDLENGTH_8B;
  huart1.Init.StopBits = UART_STOPBITS_1;
  huart1.Init.Parity = UART_PARITY_NONE;
--- a/application/chassis/chassis.c
+++ b/application/chassis/chassis.c
@ -22,6 +22,8 @@
 #include "bsp_dwt.h"
 #include "referee_UI.h"
 #include "arm_math.h"
+#include "power_meter/power_meter.h"
+#include "user_lib.h"

 /* 根据robot_def.h中的macro自动计算的参数 */
 #define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f)     // 半轴距
@ -42,11 +44,14 @@ 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 Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI
+
+

 static SuperCapInstance *cap;                                       // 超级电容
 static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
+static PowerMeterInstance *power_meter;
+float chassis_power = 0;
+

 /* 用于自旋变速策略的时间变量 */
 // static float t;
@ -54,24 +59,27 @@ static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left righ
 /* 私有函数计算的中介变量,设为静态避免参数传递的开销 */
 static float chassis_vx, chassis_vy;     // 将云台系的速度投影到底盘
 static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅
-
+float P_cmdall = 0;
+static float input=0;
 void ChassisInit()
 {
+
+
    // 四个轮子的参数一样,改tx_id和反转标志位即可
    Motor_Init_Config_s chassis_motor_config = {
        .can_init_config.can_handle = &hcan1,
        .controller_param_init_config = {
            .speed_PID = {
-                .Kp = 10, // 4.5
-                .Ki = 0,  // 0
-                .Kd = 0,  // 0
+                .Kp = 4, // 4
+                .Ki = 1.2f,  // 1.2
+                .Kd = 0.01f,  // 0.01
                .IntegralLimit = 3000,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .MaxOut = 12000,
            },
            .current_PID = {
-                .Kp = 0.5, // 0.4
-                .Ki = 0,   // 0
+                .Kp = 0.4f, // 0.4
+                .Ki = 0.1f,   // 0
                .Kd = 0,
                .IntegralLimit = 3000,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
@ -82,16 +90,18 @@ void ChassisInit()
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = SPEED_LOOP,
-            .close_loop_type = SPEED_LOOP | CURRENT_LOOP,
+            .close_loop_type = SPEED_LOOP,
+
+            .power_limit_flag = POWER_LIMIT_ON,
        },
        .motor_type = M3508,
    };
    //  @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
-    chassis_motor_config.can_init_config.tx_id = 1;
-    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
+    chassis_motor_config.can_init_config.tx_id = 3;
+    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    motor_lf = DJIMotorInit(&chassis_motor_config);

-    chassis_motor_config.can_init_config.tx_id = 2;
+    chassis_motor_config.can_init_config.tx_id = 1;
    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
    motor_rf = DJIMotorInit(&chassis_motor_config);

@ -99,19 +109,34 @@ void ChassisInit()
    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
    motor_lb = DJIMotorInit(&chassis_motor_config);

-    chassis_motor_config.can_init_config.tx_id = 3;
-    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
+    chassis_motor_config.can_init_config.tx_id = 2;
+    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 = {
-            .can_handle = &hcan2,
-            .tx_id = 0x302, // 超级电容默认接收id
-            .rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的
-        }};
+            .can_config = {
+                    .can_handle = &hcan1,
+                    .tx_id = 0x210,
+                    .rx_id = 0x211,
+            },
+            .buffer_pid_config = {
+                    .Kp = 0.7f,
+                    .Ki = 0,
+                    .Kd = 0,
+                    .MaxOut = 300,
+            }
+    };
    cap = SuperCapInit(&cap_conf); // 超级电容初始化
+    SuperCapSetPower(cap,70);  // 超级电容限制功率
+
+//    PowerMeter_Init_Config_s power_conf = {
+//            .can_config = {
+//                    .can_handle = &hcan2,
+//                    .rx_id = 0x211,
+//            }
+//    };
+//    power_meter = PowerMeterInit(&power_conf);

    // 发布订阅初始化,如果为双板,则需要can comm来传递消息
 #ifdef CHASSIS_BOARD
@ -134,7 +159,7 @@ void ChassisInit()
    chassis_pub = PubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s));
 #endif // ONE_BOARD
 }
-
+//225+0+215-0
 #define LF_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
 #define RF_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
 #define LB_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
@ -145,29 +170,77 @@ void ChassisInit()
 */
 static void MecanumCalculate()
 {
-    vt_lf = -chassis_vx - chassis_vy - chassis_cmd_recv.wz * LF_CENTER;
-    vt_rf = -chassis_vx + chassis_vy - chassis_cmd_recv.wz * RF_CENTER;
-    vt_lb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * LB_CENTER;
-    vt_rb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * RB_CENTER;
+    vt_lf = chassis_vx + chassis_vy - chassis_cmd_recv.wz * (LF_CENTER);
+    vt_rf = -chassis_vx + chassis_vy + chassis_cmd_recv.wz * (RF_CENTER);
+    vt_lb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * (-LB_CENTER);
+    vt_rb = -chassis_vx - chassis_vy + chassis_cmd_recv.wz * (-RB_CENTER);
 }

+
 /**
 * @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值
 *
 */
 static void LimitChassisOutput()
 {
-    // 功率限制待添加
-    // referee_data->PowerHeatData.chassis_power;
-    // referee_data->PowerHeatData.chassis_power_buffer;
+    static float P_max=0;
+    float initial_total_power[4]={motor_rf->motor_controller.motor_power_predict ,
+                                  motor_rb->motor_controller.motor_power_predict ,
+                                  motor_lb->motor_controller.motor_power_predict ,
+                                  motor_lf->motor_controller.motor_power_predict};
+    P_cmdall = 0;
+    for(uint8_t i=0;i<4;i++)
+    {
+        if(initial_total_power[i]<0)
+            continue;
+        P_cmdall += initial_total_power[i];
+    }
+
+    if (cap->cap_msg.cap_vol>1800)
+    {
+         P_max = input + chassis_cmd_recv.P_SuperCap ;
+    }
+    else
+    {
+        P_max = input;
+    }
+    float K = P_max/P_cmdall;
+
+
+    motor_rf->motor_controller.motor_power_scale = K;
+    motor_rb->motor_controller.motor_power_scale = K;
+    motor_lf->motor_controller.motor_power_scale = K;
+    motor_lb->motor_controller.motor_power_scale = K;
+
+    {
+        DJIMotorSetRef(motor_lf, vt_lf);
+        DJIMotorSetRef(motor_rf, vt_rf);
+        DJIMotorSetRef(motor_lb, vt_lb);
+        DJIMotorSetRef(motor_rb, vt_rb);
+    }
+
+

-    // 完成功率限制后进行电机参考输入设定
-    DJIMotorSetRef(motor_lf, vt_lf);
-    DJIMotorSetRef(motor_rf, vt_rf);
-    DJIMotorSetRef(motor_lb, vt_lb);
-    DJIMotorSetRef(motor_rb, vt_rb);
 }
+/**
+ * @brief 每次随等级更新超电的设定功率
+ *
+ */
+static void SuperCapSetUpdate()
+{

+    PIDCalculate(&cap->buffer_pid, chassis_cmd_recv.chassis_power_buffer,30);
+    input = chassis_cmd_recv.chassis_power_limit - cap->buffer_pid.Output;
+    LIMIT_MIN_MAX(input, 30, 130);
+    SuperCapSetPower(cap,input);
+
+
+//    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的数据
@ -191,7 +264,6 @@ void ChassisTask()
 #ifdef CHASSIS_BOARD
    chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
 #endif // CHASSIS_BOARD
-
    if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
    { // 如果出现重要模块离线或遥控器设置为急停,让电机停止
        DJIMotorStop(motor_lf);
@ -214,11 +286,29 @@ void ChassisTask()
        chassis_cmd_recv.wz = 0;
        break;
    case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
-        chassis_cmd_recv.wz = -1.5f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
+        chassis_cmd_recv.wz = 2.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
+        if(chassis_cmd_recv.wz > 4500)
+            chassis_cmd_recv.wz = 4500;
+        if(chassis_cmd_recv.wz < -4500)
+            chassis_cmd_recv.wz = -4500;
        break;
    case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
-        chassis_cmd_recv.wz = 4000;
+        chassis_cmd_recv.wz = 4500;
        break;
+    case CHASSIS_VERTICAL_YAW:
+        chassis_cmd_recv.offset_angle -= 45;
+        chassis_cmd_recv.wz = 2.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
+        if(chassis_cmd_recv.wz > 4500)
+            chassis_cmd_recv.wz = 4500;
+        if(chassis_cmd_recv.wz < -4500)
+            chassis_cmd_recv.wz = -4500;
+        break;
+    case CHASSIS_FIXED:
+        chassis_cmd_recv.vx=0;
+        chassis_cmd_recv.vy=0;
+        chassis_cmd_recv.wz = 0;
+        break;
+
    default:
        break;
    }
@ -228,25 +318,37 @@ void ChassisTask()
    static float sin_theta, cos_theta;
    cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
    sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
+
    chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
    chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;

+
+
+
+
    // 根据控制模式进行正运动学解算,计算底盘输出
    MecanumCalculate();
+    //读取底盘功率，方便功率控制
+//    chassis_power = PowerMeterGet(power_meter);

+    //更新超电设定值
+    SuperCapSetUpdate();
    // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
    LimitChassisOutput();
-
    // 根据电机的反馈速度和IMU(如果有)计算真实速度
    EstimateSpeed();

+
+
    // // 获取裁判系统数据   建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
    // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
-    // chassis_feedback_data.enemy_color = referee_data->GameRobotState.robot_id > 7 ? 1 : 0;
+    //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;
    // chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;

+    chassis_feedback_data.cap_vol = cap->cap_msg.cap_vol;
    // 推送反馈消息
 #ifdef ONE_BOARD
    PubPushMessage(chassis_pub, (void *)&chassis_feedback_data);
--- a/application/cmd/robot_cmd.c
+++ b/application/cmd/robot_cmd.c
@ -3,17 +3,21 @@
 #include "robot_cmd.h"
 // module
 #include "remote_control.h"
+#include "referee_task.h"
 #include "ins_task.h"
 #include "master_process.h"
 #include "message_center.h"
 #include "general_def.h"
 #include "dji_motor.h"
+#include "dm4310.h"
+#include "auto_aim.h"
+#include "vision_transfer.h"
 // bsp
 #include "bsp_dwt.h"
 #include "bsp_log.h"

 // 私有宏,自动将编码器转换成角度值
-#define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360
+#define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD *  RAD_2_DEGREE) // 对齐时的角度,0-360,使用达妙电机
 #define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF_DJI) // pitch水平时电机的角度,0-360

 /* cmd应用包含的模块实例指针和交互信息存储*/
@ -25,13 +29,25 @@ 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_yaw_err; //for test
+static int temp_index; //for test

 static Chassis_Ctrl_Cmd_s chassis_cmd_send;      // 发送给底盘应用的信息,包括控制信息和UI绘制相关
 static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等

 static RC_ctrl_t *rc_data;              // 遥控器数据,初始化时返回
-static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回
-static Vision_Send_s vision_send_data;  // 视觉发送数据
+//static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回
+//static Vision_Send_s vision_send_data;  // 视觉发送数据
+static VT_ctrl_t *vt_data;               // 图传链路下发的键鼠遥控数据 与遥控器数据格式保持一致
+RecievePacket_t *vision_recv_data; // 视觉接收数据指针,初始化时返回
+SendPacket_t vision_send_data;  // 视觉发送数据
+
+//自瞄相关信息
+Trajectory_Type_t trajectory_cal;
+Aim_Select_Type_t aim_select;
+static uint32_t no_find_cnt; // 未发现目标计数
+static uint8_t auto_aim_flag = 0; //辅助瞄准标志位  视野内有目标开启 目标丢失关闭

 static Publisher_t *gimbal_cmd_pub;            // 云台控制消息发布者
 static Subscriber_t *gimbal_feed_sub;          // 云台反馈信息订阅者
@ -44,12 +60,19 @@ 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; // 用于获取裁判系统的数据
+static referee_info_vt_t* referee_vt_data;
+static uint8_t Pitch_Limit_Flag=1;
+
+float PitchMotorAngle;
+float DeltaPitchAngle;

 void RobotCMDInit()
 {
    rc_data = RemoteControlInit(&huart3);   // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
-    vision_recv_data = VisionInit(&huart1); // 视觉通信串口
-
+    vision_recv_data = VisionInit(); // 视觉通信,用的是USB通讯
+    //vt_data = VTRefereeInit(&huart1);
+    //@TODO:找一个可以自由切换图传链路和遥控器控制的逻辑
    gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
    gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
    shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
@ -71,7 +94,9 @@ void RobotCMDInit()
    };
    cmd_can_comm = CANCommInit(&comm_conf);
 #endif // GIMBAL_BOARD
+    referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI
    gimbal_cmd_send.pitch = 0;
+    shoot_cmd_send.motor_speed = 5500;

    robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入
 }
@ -101,7 +126,115 @@ static void CalcOffsetAngle()
    else
        chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE + 360.0f;
 #endif
+//    if(chassis_cmd_send.offset_angle > 340.0f)
+//        chassis_cmd_send.offset_angle = chassis_cmd_send.offset_angle - 360.0f;
+//    else if(chassis_cmd_send.offset_angle < -340.0f)
+//        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.current_HP <= 0)
+    {
+        rc_data[TEMP].key_count[KEY_PRESS][Key_E] = 0;
+    }
+}
+
+static void update_ui_data()
+{
+    ui_data.chassis_mode = chassis_cmd_send.chassis_mode;
+    ui_data.gimbal_mode = gimbal_cmd_send.gimbal_mode;
+    ui_data.friction_mode = shoot_cmd_send.friction_mode;
+    ui_data.shoot_mode = shoot_cmd_send.shoot_mode;
+    ui_data.Chassis_Power_Data.chassis_power_mx = referee_data->GameRobotState.chassis_power_limit;
+    ui_data.Vision_fire = aim_select.suggest_fire;
+    ui_data.Chassis_Power_Data.cap_vol = chassis_fetch_data.cap_vol/100.0f;
+    ui_data.Chassis_Power_Data.motor_speed = shoot_cmd_send.motor_speed;
+    ui_data.Motor_Temperature = shoot_fetch_data.Motor_Temperature;
+}
+static void pitch_limit()
+{
+    LIMIT_MIN_MAX(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE);
+
+//    if(!Pitch_Limit_Flag)
+//    {
+//
+//        PitchMotorAngle = -0.0019f * gimbal_fetch_data.pitch_motor_total_angle - 0.4059;//电机旋转导致的云台相对底盘角度
+//        DeltaPitchAngle = gimbal_fetch_data.gimbal_imu_data.Pitch - PitchMotorAngle;//陀螺仪和电机角的误差
+//        gimbal_cmd_send.pitch = LIMIT_MIN_MAX(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE-DeltaPitchAngle, PITCH_MAX_ANGLE-DeltaPitchAngle);
+//    }
+}
+static void auto_aim_mode()
+{
+    trajectory_cal.v0 = 13.5f; //弹速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 >= 2000) {
+            //gimbal_scan_flag = 1;
+            //auto_aim_flag = 0;
+        }
+        //else
+        //auto_aim_flag = 1;
+    } else {
+        //弹道解算
+        no_find_cnt = 0;
+        auto_aim_flag = 1;
+
+        int target_index = -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]);
+
+        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 + 0.2 * trajectory_cal.dis;
+
+        float target_yaw = atan2f(aim_select.armor_pose[target_index].y, aim_select.armor_pose[target_index].x) * 180 / PI;
+
+        //float target_yaw = aim_select.armor_pose[target_index].yaw * 180 / PI;
+        //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);
+        temp_yaw_err = yaw_err;
+        if(vision_recv_data->armors_num == 3){
+            if (yaw_err <= 2) //3度
+            {
+                aim_select.suggest_fire = 1;
+            }
+            else
+                aim_select.suggest_fire = 0;
+        }else{
+            if (yaw_err <= 4) //3度
+            {
+                aim_select.suggest_fire = 1;
+            }
+            else
+                aim_select.suggest_fire = 0;
+        }
+
+
+//            float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw);
+//            if (yaw_err <= 3) //3度
+//                aim_select.suggest_fire = 1;
+//            else
+//                aim_select.suggest_fire = 0;
+    }
+}
+

 /**
 * @brief 控制输入为遥控器(调试时)的模式和控制量设置
@ -109,40 +242,50 @@ static void CalcOffsetAngle()
 */
 static void RemoteControlSet()
 {
+    aim_select.suggest_fire = 0;
    // 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据?
-    if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘跟随云台
+    if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],小陀螺
    {
        chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
        gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
+
    }
-    else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动
+    else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘跟随云台
    {
-        chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW;
-        gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
+        chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
+        gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
+        shoot_cmd_send.tele_mode = TELE_CLOSE;
    }

    // 云台参数,确定云台控制数据
-    if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[中],视觉模式
+    if (switch_is_mid(rc_data[TEMP].rc.switch_left) ||
+        (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)) // 左侧开关状态为[中],或视觉未识别到目标,纯遥控器拨杆控制
+
    {
-        // 待添加,视觉会发来和目标的误差,同样将其转化为total angle的增量进行控制
-        // ...
+        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();
+
    }
-    // 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制
-    if (switch_is_down(rc_data[TEMP].rc.switch_left) || vision_recv_data->target_state == NO_TARGET)
-    { // 按照摇杆的输出大小进行角度增量,增益系数需调整
-        gimbal_cmd_send.yaw += 0.005f * (float)rc_data[TEMP].rc.rocker_l_;
-        gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_l1;
+    // 左侧开关状态为[下],视觉模式
+    if (switch_is_down(rc_data[TEMP].rc.switch_left)) {
+        auto_aim_mode();
    }
    // 云台软件限位

    // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
-    chassis_cmd_send.vx = 10.0f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向
-    chassis_cmd_send.vy = 10.0f * (float)rc_data[TEMP].rc.rocker_r1; // 1数值方向
+    chassis_cmd_send.vx = 20.1f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向
+    chassis_cmd_send.vy = 20.1f * (float)rc_data[TEMP].rc.rocker_r1; // 1数值方向

    // 发射参数
-    if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开
-        ;                                            // 弹舱舵机控制,待添加servo_motor模块,开启
+    if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],调整水平pitch参数
+    {
+        shoot_cmd_send.tele_mode = TELE_OPEN;
+    }// 弹舱舵机控制,待添加servo_motor模块,开启
    else
+        shoot_cmd_send.tele_mode = TELE_CLOSE;
        ; // 弹舱舵机控制,待添加servo_motor模块,关闭

    // 摩擦轮控制,拨轮向上打为负,向下为正
@ -150,13 +293,15 @@ static void RemoteControlSet()
        shoot_cmd_send.friction_mode = FRICTION_ON;
    else
        shoot_cmd_send.friction_mode = FRICTION_OFF;
+    ;
+        //shoot_cmd_send.friction_mode = FRICTION_OFF;
    // 拨弹控制,遥控器固定为一种拨弹模式,可自行选择
    if (rc_data[TEMP].rc.dial < -500)
-        shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
+        shoot_cmd_send.load_mode = LOAD_1_BULLET;
    else
        shoot_cmd_send.load_mode = LOAD_STOP;
    // 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频,
-    shoot_cmd_send.shoot_rate = 8;
+    shoot_cmd_send.shoot_rate = 500;
 }

 /**
@ -165,48 +310,92 @@ static void RemoteControlSet()
 */
 static void MouseKeySet()
 {
-    chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].w * 300 - rc_data[TEMP].key[KEY_PRESS].s * 300; // 系数待测
-    chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].s * 300 - rc_data[TEMP].key[KEY_PRESS].d * 300;
-
-    gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 10; // 系数待测
-    gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 10;
-
-    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速
+    chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 60000 - rc_data[TEMP].key[KEY_PRESS].d * 60000; // 系数待测
+    chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 60000 - rc_data[TEMP].key[KEY_PRESS].s * 60000;
+    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2)
    {
-    case 0:
-        shoot_cmd_send.bullet_speed = 15;
-        break;
-    case 1:
-        shoot_cmd_send.bullet_speed = 18;
-        break;
-    default:
-        shoot_cmd_send.bullet_speed = 30;
-        break;
+        case 0:
+            gimbal_cmd_send.yaw -= (float)rc_data[TEMP].mouse.x / 660; // 系数待测
+            gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660;
+            break;
+        case 1 :
+            gimbal_cmd_send.yaw -= (float)rc_data[TEMP].mouse.x / 660 * 3; // 系数待测
+            gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 3;
+            break;
    }
-    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 4) // E键设置发射模式
+
+
+    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.load_mode = LOAD_1_BULLET;
+        }
+
+    } else if ((!rc_data[TEMP].mouse.press_l) && (!rc_data[TEMP].mouse.press_r))
    {
-    case 0:
        shoot_cmd_send.load_mode = LOAD_STOP;
-        break;
-    case 1:
-        shoot_cmd_send.load_mode = LOAD_1_BULLET;
-        break;
-    case 2:
-        shoot_cmd_send.load_mode = LOAD_3_BULLET;
-        break;
-    default:
-        shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
-        break;
+
    }
-    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键开关弹舱
+
+    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[KEY_PRESS].v) // V键开启连发//
    {
    case 0:
-        shoot_cmd_send.lid_mode = LID_OPEN;
+        shoot_cmd_send.shoot_rate= 500;
+        break;
+    case 1:
+        shoot_cmd_send.shoot_rate = 50;
+        break;
+    }
+
+    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键手动刷新UI
+    {
+    case 1:
+        MyUIInit();
+        rc_data[TEMP].key_count[KEY_PRESS][Key_R]++;
        break;
    default:
-        shoot_cmd_send.lid_mode = LID_CLOSE;
        break;
    }
+    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_B] % 2) // B键加100转速
+    {
+        case 1:
+            shoot_cmd_send.motor_speed +=100;
+            rc_data[TEMP].key_count[KEY_PRESS][Key_B]++;
+            break;
+        default:
+            break;
+    }
+    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_C] % 2) // C键减100转速
+    {
+        case 1:
+            shoot_cmd_send.motor_speed -=100;
+            rc_data[TEMP].key_count[KEY_PRESS][Key_C]++;
+            break;
+        default:
+            break;
+    }
    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮
    {
    case 0:
@ -216,32 +405,170 @@ static void MouseKeySet()
        shoot_cmd_send.friction_mode = FRICTION_ON;
        break;
    }
-    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_C] % 4) // C键设置底盘速度
+    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q键开关倍镜
    {
-    case 0:
-        chassis_cmd_send.chassis_speed_buff = 40;
-        break;
-    case 1:
-        chassis_cmd_send.chassis_speed_buff = 60;
-        break;
-    case 2:
-        chassis_cmd_send.chassis_speed_buff = 80;
-        break;
-    default:
-        chassis_cmd_send.chassis_speed_buff = 100;
-        break;
+        case 0:
+            shoot_cmd_send.tele_mode = TELE_CLOSE;
+            break;
+        case 1:
+            shoot_cmd_send.tele_mode = TELE_OPEN;
+            break;
    }
-    switch (rc_data[TEMP].key[KEY_PRESS].shift) // 待添加 按shift允许超功率 消耗缓冲能量
+    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺
    {
-    case 1:
-
-        break;
-
-    default:
-
-        break;
+        case 0:
+            chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
+            break;
+        default:
+            chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
+            break;
    }
+    switch (rc_data[TEMP].key[KEY_PRESS].z) // Z键开关吊射模式
+    {
+        case 1:
+            chassis_cmd_send.chassis_mode = CHASSIS_VERTICAL_YAW;
+            //chassis_cmd_send.vx=chassis_cmd_send.vy=0;  //吊射时底盘不动
+            break;
+    }
+    switch (rc_data[TEMP].key[KEY_PRESS].shift) //shift开关超电//
+    {
+        case 0:
+            chassis_cmd_send.P_SuperCap=5;
+            break;
+        case 1:
+            chassis_cmd_send.P_SuperCap=200;
+            break;
+    }
+    pitch_limit();
 }
+/**
+	 * @brief 输入为（图传链路）键鼠时模式和控制量设置
+ *
+ */
+static void VTMouseKeySet()
+{
+    chassis_cmd_send.vx = vt_data[TEMP].key[KEY_PRESS].a * 10000 - vt_data[TEMP].key[KEY_PRESS].d * 10000; // 系数待测
+    chassis_cmd_send.vy = vt_data[TEMP].key[KEY_PRESS].w * 10000 - vt_data[TEMP].key[KEY_PRESS].s * 10000;
+
+    gimbal_cmd_send.yaw -= (float)vt_data[TEMP].mouse.x / 660 * 10; // 系数待测
+    gimbal_cmd_send.pitch += (float)vt_data[TEMP].mouse.y / 660 * 10;
+
+
+    aim_select.suggest_fire = 0;
+    if (vt_data[TEMP].mouse.press_l && (!vt_data[TEMP].mouse.press_r)) // 左键发射模式
+    {
+        if (shoot_cmd_send.friction_mode == FRICTION_ON) {
+            shoot_cmd_send.load_mode = LOAD_1_BULLET;
+        }
+
+    } else if ((!vt_data[TEMP].mouse.press_l) && (!vt_data[TEMP].mouse.press_r))
+    {
+        shoot_cmd_send.load_mode = LOAD_STOP;
+
+    }
+
+    if (vt_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 && vt_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 (vt_data[TEMP].key[KEY_PRESS].v) // V键开启连发//
+    {
+        case 0:
+            shoot_cmd_send.shoot_rate= 500;
+            break;
+        case 1:
+            shoot_cmd_send.shoot_rate = 50;
+            break;
+    }
+
+    switch (vt_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键手动刷新UI
+    {
+        case 1:
+            MyUIInit();
+            vt_data[TEMP].key_count[KEY_PRESS][Key_R]++;
+            break;
+        default:
+            break;
+    }
+    switch (vt_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮
+    {
+        case 0:
+            shoot_cmd_send.friction_mode = FRICTION_OFF;
+            break;
+        default:
+            shoot_cmd_send.friction_mode = FRICTION_ON;
+            break;
+    }
+    switch (vt_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q键开关倍镜
+    {
+        case 0:
+            shoot_cmd_send.tele_mode = TELE_CLOSE;
+            break;
+        case 1:
+            shoot_cmd_send.tele_mode = TELE_OPEN;
+            break;
+    }
+    switch (vt_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺
+    {
+        case 0:
+            if(!vt_data[TEMP].key[KEY_PRESS].z)
+            {
+                chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
+                gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
+            }
+            else
+            {
+                chassis_cmd_send.chassis_mode = CHASSIS_VERTICAL_YAW;
+                gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
+            }
+            break;
+        default:
+            chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
+            gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
+            break;
+
+    }
+//    switch (vt_data[TEMP].key[KEY_PRESS].z) // Z键侧身用于瞄准
+//    {
+//        case 0:
+//            chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
+//            break;
+//        case 1:
+//            chassis_cmd_send.chassis_mode = CHASSIS_VERTICAL_YAW;
+//            break;
+//    }
+    switch (vt_data[TEMP].key[KEY_PRESS].shift) // 待添加 按shift允许超功率 消耗缓冲能量
+    {
+        case 1:
+
+            break;
+
+        default:
+
+            break;
+    }
+
+    pitch_limit();
+
+}
+

 /**
 * @brief  紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等
@ -266,8 +593,13 @@ static void EmergencyHandler()
    // 遥控器右侧开关为[上],恢复正常运行
    if (switch_is_up(rc_data[TEMP].rc.switch_right))
    {
+
        robot_state = ROBOT_READY;
+        gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
        shoot_cmd_send.shoot_mode = SHOOT_ON;
+        shoot_cmd_send.friction_mode = FRICTION_OFF;
+
+
        LOGINFO("[CMD] reinstate, robot ready");
    }
 }
@ -284,20 +616,27 @@ void RobotCMDTask()
 #endif // GIMBAL_BOARD
    SubGetMessage(shoot_feed_sub, &shoot_fetch_data);
    SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);
-
+    //更新UI
+    update_ui_data();
    // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成
    CalcOffsetAngle();
    // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠
-    if (switch_is_down(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制
+    if (switch_is_down(rc_data[TEMP].rc.switch_left) ||
+        switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],[中],遥控器控制
        RemoteControlSet();
    else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制
+    {
        MouseKeySet();
+    }

    EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况

    // 设置视觉发送数据,还需增加加速度和角速度数据
-    // VisionSetFlag(chassis_fetch_data.enemy_color,,chassis_fetch_data.bullet_speed)
-
+    VisionSetFlag(!referee_data->referee_id.Robot_Color);
+    //根据裁判系统反馈确定底盘功率上限和缓冲功率
+    chassis_cmd_send.chassis_power_limit = referee_data->GameRobotState.chassis_power_limit;
+    chassis_cmd_send.chassis_power_buffer = referee_data->PowerHeatData.buffer_energy;
+    death_check();
    // 推送消息,双板通信,视觉通信等
    // 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置
 #ifdef ONE_BOARD
@ -308,5 +647,4 @@ void RobotCMDTask()
 #endif // GIMBAL_BOARD
    PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
    PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
-    VisionSend(&vision_send_data);
 }
--- a/application/cmd/robot_cmd.h
+++ b/application/cmd/robot_cmd.h
@ -2,6 +2,9 @@
 #define ROBOT_CMD_H


+
+
+
 /**
 * @brief 机器人核心控制任务初始化,会被RobotInit()调用
 * 
--- a/application/gimbal/gimbal.c
+++ b/application/gimbal/gimbal.c
@ -6,39 +6,41 @@
 #include "general_def.h"

 #include "bmi088.h"
+#include "dm4310.h"

 static attitude_t *gimba_IMU_data; // 云台IMU数据
-static DJIMotorInstance *yaw_motor, *pitch_motor;
+static DJIMotorInstance  *pitch_motor;
+static DMMotorInstance *yaw_motor;

 static Publisher_t *gimbal_pub;                   // 云台应用消息发布者(云台反馈给cmd)
 static Subscriber_t *gimbal_sub;                  // cmd控制消息订阅者
 static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
 static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;         // 来自cmd的控制信息

+
 void GimbalInit()
 {
    gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
    // YAW
    Motor_Init_Config_s yaw_config = {
        .can_init_config = {
-            .can_handle = &hcan1,
-            .tx_id = 1,
+            .can_handle = &hcan2,
+            .tx_id = 0x01,
+            .rx_id = 0x43,
        },
        .controller_param_init_config = {
            .angle_PID = {
-                .Kp = 8, // 8
+                .Kp = 2.0f,//0.5, // 8
                .Ki = 0,
-                .Kd = 0,
-                .DeadBand = 0.1,
+                .Kd = 0.02f,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .IntegralLimit = 100,
-
                .MaxOut = 500,
            },
            .speed_PID = {
-                .Kp = 50,  // 50
-                .Ki = 200, // 200
-                .Kd = 0,
+                .Kp = 3,//0.5f,
+                .Ki = 0,//0.2f,
+                .Kd = 0,//10
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .IntegralLimit = 3000,
                .MaxOut = 20000,
@ -51,10 +53,11 @@ void GimbalInit()
            .angle_feedback_source = OTHER_FEED,
            .speed_feedback_source = OTHER_FEED,
            .outer_loop_type = ANGLE_LOOP,
-            .close_loop_type = ANGLE_LOOP | SPEED_LOOP,
+            .close_loop_type =  ANGLE_LOOP |SPEED_LOOP,
            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
        },
-        .motor_type = GM6020};
+        .motor_type = DM4310
+    };
    // PITCH
    Motor_Init_Config_s pitch_config = {
        .can_init_config = {
@ -63,17 +66,17 @@ void GimbalInit()
        },
        .controller_param_init_config = {
            .angle_PID = {
-                .Kp = 10, // 10
+                .Kp = 0.35f,
                .Ki = 0,
-                .Kd = 0,
+                .Kd = 0.005f,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
-                .IntegralLimit = 100,
-                .MaxOut = 500,
+                .IntegralLimit = 100,//100
+                .MaxOut = 500,//500
            },
            .speed_PID = {
-                .Kp = 50,  // 50
-                .Ki = 350, // 350
-                .Kd = 0,   // 0
+                .Kp = 6000,
+                .Ki = 1000, // 350
+                .Kd = 0,//15000,   // 0
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .IntegralLimit = 2500,
                .MaxOut = 20000,
@ -87,18 +90,27 @@ void GimbalInit()
            .speed_feedback_source = OTHER_FEED,
            .outer_loop_type = ANGLE_LOOP,
            .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
-            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
+            .motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
+            .feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE,
        },
-        .motor_type = GM6020,
+        .motor_type = M3508,
    };
+
    // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
-    yaw_motor = DJIMotorInit(&yaw_config);
+    yaw_motor = DMMotorInit(&yaw_config);
    pitch_motor = DJIMotorInit(&pitch_config);

    gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
    gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
 }

+static void YawMotorEnable(void)
+{
+    if(yaw_motor->measure.state == 0)//如果电机处于失能状态
+    {
+        DMMotorSetMode(DM_CMD_MOTOR_MODE, yaw_motor);//发送使能命令
+    }
+}
 /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
 void GimbalTask()
 {
@ -112,30 +124,29 @@ void GimbalTask()
    {
    // 停止
    case GIMBAL_ZERO_FORCE:
-        DJIMotorStop(yaw_motor);
+        DMMotorStop(yaw_motor);
        DJIMotorStop(pitch_motor);
        break;
    // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用
    case GIMBAL_GYRO_MODE: // 后续只保留此模式
-        DJIMotorEnable(yaw_motor);
+        YawMotorEnable();
+        DMMotorEnable(yaw_motor);
        DJIMotorEnable(pitch_motor);
-        DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);
-        DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED);
        DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED);
        DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
-        DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
+        DMMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
        DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
        break;
    // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关
    case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快)
-        DJIMotorEnable(yaw_motor);
-        DJIMotorEnable(pitch_motor);
-        DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);
-        DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED);
-        DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED);
-        DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
-        DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
-        DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
+//        DJIMotorEnable(yaw_motor);
+//        DJIMotorEnable(pitch_motor);
+//        DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);
+//        DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED);
+//        DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED);
+//        DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
+//        DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
+//        DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
        break;
    default:
        break;
@ -148,7 +159,8 @@ 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.pitch_motor_total_angle = pitch_motor->measure.total_angle;
    // 推送消息
+
    PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
 }
--- a/application/robot.c
+++ b/application/robot.c
@ -29,6 +29,8 @@ void RobotInit()
    
    BSPInit();

+
+
 #if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
    RobotCMDInit();
    GimbalInit();
@ -39,6 +41,7 @@ void RobotInit()
    ChassisInit();
 #endif

+
    OSTaskInit(); // 创建基础任务

    // 初始化完成,开启中断
--- a/application/robot_def.h
+++ b/application/robot_def.h
@ -26,21 +26,21 @@

 /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
 // 云台参数
-#define YAW_CHASSIS_ALIGN_ECD 2711  // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
+#define YAW_CHASSIS_ALIGN_ECD 5.482276  // 云台和底盘对齐指向相同方向时的电机position值,需要加上PI，因为单圈角度计算里增加了PI若对云台有机械改动需要修改
 #define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
 #define PITCH_HORIZON_ECD 3412      // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
-#define PITCH_MAX_ANGLE 0           // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
-#define PITCH_MIN_ANGLE 0           // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
+#define PITCH_MAX_ANGLE 15           // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
+#define PITCH_MIN_ANGLE -35           // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
 // 发射参数
-#define ONE_BULLET_DELTA_ANGLE 36    // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
-#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
-#define NUM_PER_CIRCLE 10            // 拨盘一圈的装载量
+#define ONE_BULLET_DELTA_ANGLE 1933.272    // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
+#define REDUCTION_RATIO_LOADER 27.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f//初始是49，增加为27
+#define NUM_PER_CIRCLE 5            // 拨盘一圈的装载量
 // 机器人底盘修改的参数,单位为mm(毫米)
-#define WHEEL_BASE 350              // 纵向轴距(前进后退方向)
-#define TRACK_WIDTH 300             // 横向轮距(左右平移方向)
+#define WHEEL_BASE 433.86           // 纵向轴距(前进后退方向)
+#define TRACK_WIDTH 463.92            // 横向轮距(左右平移方向)
 #define CENTER_GIMBAL_OFFSET_X 0    // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0
 #define CENTER_GIMBAL_OFFSET_Y 0    // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0
-#define RADIUS_WHEEL 60             // 轮子半径
+#define RADIUS_WHEEL 75             // 轮子半径
 #define REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换

 #define GYRO2GIMBAL_DIR_YAW 1   // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反
@ -86,6 +86,8 @@ typedef enum
    CHASSIS_ROTATE,            // 小陀螺模式
    CHASSIS_NO_FOLLOW,         // 不跟随，允许全向平移
    CHASSIS_FOLLOW_GIMBAL_YAW, // 跟随模式，底盘叠加角度环控制
+    CHASSIS_VERTICAL_YAW,
+    CHASSIS_FIXED,
 } chassis_mode_e;

 // 云台模式设置
@ -94,6 +96,7 @@ typedef enum
    GIMBAL_ZERO_FORCE = 0, // 电流零输入
    GIMBAL_FREE_MODE,      // 云台自由运动模式,即与底盘分离(底盘此时应为NO_FOLLOW)反馈值为电机total_angle;似乎可以改为全部用IMU数据?
    GIMBAL_GYRO_MODE,      // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式
+    ADJUST_PITCH_MODE,      //找pitch的限位
 } gimbal_mode_e;

 // 发射模式设置
@ -110,9 +113,9 @@ typedef enum

 typedef enum
 {
-    LID_OPEN = 0, // 弹舱盖打开
-    LID_CLOSE,    // 弹舱盖关闭
-} lid_mode_e;
+    TELE_CLOSE = 0, // 倍镜打开
+    TELE_OPEN,    // 倍镜关闭
+} tele_mode_e;

 typedef enum
 {
@ -126,7 +129,9 @@ typedef enum
 // 功率限制,从裁判系统获取,是否有必要保留?
 typedef struct
 { // 功率控制
-    float chassis_power_mx;
+    uint16_t chassis_power_mx;
+    float cap_vol;
+    uint16_t motor_speed;
 } Chassis_Power_Data_s;

 /* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */
@ -143,7 +148,9 @@ typedef struct
    float wz;           // 旋转速度
    float offset_angle; // 底盘和归中位置的夹角
    chassis_mode_e chassis_mode;
-    int chassis_speed_buff;
+    int chassis_power_buffer;
+    uint16_t chassis_power_limit;
+    uint8_t P_SuperCap;
    // UI部分
    //  ...

@ -155,6 +162,8 @@ typedef struct
    float yaw;
    float pitch;
    float chassis_rotate_wz;
+    uint8_t YawMotorEnableFlag;
+

    gimbal_mode_e gimbal_mode;
 } Gimbal_Ctrl_Cmd_s;
@ -164,11 +173,13 @@ typedef struct
 {
    shoot_mode_e shoot_mode;
    loader_mode_e load_mode;
-    lid_mode_e lid_mode;
+    tele_mode_e tele_mode;
    friction_mode_e friction_mode;
-    Bullet_Speed_e bullet_speed; // 弹速枚举
+    float bullet_speed; // 发射周期
    uint8_t rest_heat;
    float shoot_rate; // 连续发射的射频,unit per s,发/秒
+    uint16_t motor_speed;
+
 } Shoot_Ctrl_Cmd_s;

 /* ----------------gimbal/shoot/chassis发布的反馈数据----------------*/
@ -186,9 +197,8 @@ typedef struct
    // float real_vx;
    // float real_vy;
    // float real_wz;
-
+    float cap_vol;
    uint8_t rest_heat;           // 剩余枪口热量
-    Bullet_Speed_e bullet_speed; // 弹速限制
    Enemy_Color_e enemy_color;   // 0 for blue, 1 for red

 } Chassis_Upload_Data_s;
@ -197,13 +207,16 @@ typedef struct
 typedef struct
 {
    attitude_t gimbal_imu_data;
-    uint16_t yaw_motor_single_round_angle;
+    float yaw_motor_single_round_angle;
+    float pitch_motor_total_angle;
 } Gimbal_Upload_Data_s;

 typedef struct
 {
    // code to go here
    // ...
+    uint8_t stalled_flag;  //堵转标志
+    uint8_t Motor_Temperature;
 } Shoot_Upload_Data_s;

 #pragma pack() // 开启字节对齐,结束前面的#pragma pack(1)
--- a/application/robot_task.h
+++ b/application/robot_task.h
@ -16,6 +16,7 @@
 #include "buzzer.h"

 #include "bsp_log.h"
+#include "dm4310.h"

 osThreadId insTaskHandle;
 osThreadId robotTaskHandle;
@ -51,7 +52,7 @@ void OSTaskInit()
    osThreadDef(uitask, StartUITASK, osPriorityNormal, 0, 512);
    uiTaskHandle = osThreadCreate(osThread(uitask), NULL);

-    HTMotorControlInit(); // 没有注册HT电机则不会执行
+    DMMotorControlInit(); // 没有注册DM电机则不会执行
 }

 __attribute__((noreturn)) void StartINSTASK(void const *argument)
--- a/application/shoot/shoot.c
+++ b/application/shoot/shoot.c
@ -4,35 +4,44 @@
 #include "dji_motor.h"
 #include "message_center.h"
 #include "bsp_dwt.h"
+#include "referee_task.h"
 #include "general_def.h"
+#include "servo_motor.h"
+

 /* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */
-static DJIMotorInstance *friction_l, *friction_r, *loader; // 拨盘电机
-// static servo_instance *lid; 需要增加弹舱盖
+static DJIMotorInstance *friction_l, *friction_r,*friction_z, *loader; // 拨盘电机
+ static ServoInstance *telescope; //需要增加弹舱盖

 static Publisher_t *shoot_pub;
 static Shoot_Ctrl_Cmd_s shoot_cmd_recv; // 来自cmd的发射控制信息
 static Subscriber_t *shoot_sub;
 static Shoot_Upload_Data_s shoot_feedback_data; // 来自cmd的发射控制信息
-
+float Kp=2;
+float Ki=1;
+float Kd=0;
+static float stop_location;
 // dwt定时,计算冷却用
 static float hibernate_time = 0, dead_time = 0;
+uint16_t angle=2;
+static uint16_t locked_time;
+

 void ShootInit()
 {
-    // 左摩擦轮
+    // 摩擦轮
    Motor_Init_Config_s friction_config = {
        .can_init_config = {
            .can_handle = &hcan2,
        },
        .controller_param_init_config = {
            .speed_PID = {
-                .Kp = 0, // 20
+                .Kp = 6, // 20
                .Ki = 0, // 1
                .Kd = 0,
                .Improve = PID_Integral_Limit,
                .IntegralLimit = 10000,
-                .MaxOut = 15000,
+                .MaxOut = 28000,
            },
            .current_PID = {
                .Kp = 0, // 0.7
@ -48,38 +57,43 @@ void ShootInit()
            .speed_feedback_source = MOTOR_FEED,

            .outer_loop_type = SPEED_LOOP,
-            .close_loop_type = SPEED_LOOP | CURRENT_LOOP,
-            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
+            .close_loop_type = SPEED_LOOP ,
+            .motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
        },
        .motor_type = M3508};
-    friction_config.can_init_config.tx_id = 1,
-    friction_l = DJIMotorInit(&friction_config);
+//    friction_config.can_init_config.tx_id = 3,
+//    friction_l = DJIMotorInit(&friction_config);

-    friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行
-    friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
+    friction_config.can_init_config.tx_id = 4; // 右摩擦轮,改txid和方向就行
+    friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    friction_r = DJIMotorInit(&friction_config);

+    friction_config.can_init_config.tx_id = 6; // 上摩擦轮,改txid和方向就行
+    friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
+    friction_z = DJIMotorInit(&friction_config);
+
+
    // 拨盘电机
    Motor_Init_Config_s loader_config = {
        .can_init_config = {
-            .can_handle = &hcan2,
-            .tx_id = 3,
+            .can_handle = &hcan1,
+            .tx_id = 5,
        },
        .controller_param_init_config = {
            .angle_PID = {
                // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
-                .Kp = 0, // 10
+                .Kp = 70, // 10
                .Ki = 0,
-                .Kd = 0,
-                .MaxOut = 200,
+                .Kd = 0.01f,
+                .MaxOut = 15000,
            },
            .speed_PID = {
-                .Kp = 0, // 10
+                .Kp = 3.8f, // 10
                .Ki = 0, // 1
                .Kd = 0,
                .Improve = PID_Integral_Limit,
                .IntegralLimit = 5000,
-                .MaxOut = 5000,
+                .MaxOut = 30000,
            },
            .current_PID = {
                .Kp = 0, // 0.7
@ -87,47 +101,70 @@ void ShootInit()
                .Kd = 0,
                .Improve = PID_Integral_Limit,
                .IntegralLimit = 5000,
-                .MaxOut = 5000,
+                .MaxOut = 15000,
            },
        },
        .controller_setting_init_config = {
-            .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
+            .angle_feedback_source = MOTOR_FEED,
+            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
-            .close_loop_type = CURRENT_LOOP | SPEED_LOOP,
+            .close_loop_type =   SPEED_LOOP | ANGLE_LOOP,
            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
        },
-        .motor_type = M2006 // 英雄使用m3508
+        .motor_type = M3508 // 英雄使用m3508
    };
    loader = DJIMotorInit(&loader_config);

+    // 倍镜舵机
+    Servo_Init_Config_s tele_config = {
+            .htim = &htim1,
+            .Channel = TIM_CHANNEL_1,
+            .Servo_type = Servo270,
+            .Servo_Angle_Type = Free_Angle_mode,
+    };
+
+    telescope = ServoInit(&tele_config);
+
    shoot_pub = PubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s));
    shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
 }
-
+static void Stop_Check()
+{
+    if(loader->measure.total_angle < stop_location*0.9)
+    {
+        DJIMotorOuterLoop(loader, ANGLE_LOOP);
+        DJIMotorSetRef(loader,stop_location);
+        hibernate_time = DWT_GetTimeline_ms();                                             // 记录触发指令的时间
+        dead_time = 1000;
+    }
+}
 /* 机器人发射机构控制核心任务 */
 void ShootTask()
 {
    // 从cmd获取控制数据
    SubGetMessage(shoot_sub, &shoot_cmd_recv);
-
+    friction_z->motor_controller.speed_PID.Kp=friction_r->motor_controller.speed_PID.Kp = Kp;
+    friction_z->motor_controller.speed_PID.Ki=friction_r->motor_controller.speed_PID.Ki = Ki;
+    friction_z->motor_controller.speed_PID.Kd=friction_r->motor_controller.speed_PID.Kd = Kd;
    // 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机(紧急停止)
    if (shoot_cmd_recv.shoot_mode == SHOOT_OFF)
    {
-        DJIMotorStop(friction_l);
+//        DJIMotorStop(friction_l);
        DJIMotorStop(friction_r);
+        DJIMotorStop(friction_z);
        DJIMotorStop(loader);
    }
    else // 恢复运行
    {
-        DJIMotorEnable(friction_l);
+//        DJIMotorEnable(friction_l);
        DJIMotorEnable(friction_r);
+        DJIMotorEnable(friction_z);
        DJIMotorEnable(loader);
    }
-
    // 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可
    // 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发)
-    // if (hibernate_time + dead_time > DWT_GetTimeline_ms())
-    //     return;
+    if (hibernate_time + dead_time > DWT_GetTimeline_ms())
+        return;

    // 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换
    switch (shoot_cmd_recv.load_mode)
@ -139,12 +176,12 @@ void ShootTask()
        break;
    // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
    case LOAD_1_BULLET:                                                                     // 激活能量机关/干扰对方用,英雄用.
-        DJIMotorOuterLoop(loader, ANGLE_LOOP);                                              // 切换到角度环
+        DJIMotorOuterLoop(loader, ANGLE_LOOP);                                              // 切换到角度环v
        DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
-        hibernate_time = DWT_GetTimeline_ms();                                              // 记录触发指令的时间
-        dead_time = 150;                                                                    // 完成1发弹丸发射的时间
+        hibernate_time = DWT_GetTimeline_ms();                                             // 记录触发指令的时间
+        dead_time = shoot_cmd_recv.shoot_rate;                                                                    // 完成1发弹丸发射的时间
        break;
-    // 三连发,如果不需要后续可能删除
+    // 自爆连发
    case LOAD_3_BULLET:
        DJIMotorOuterLoop(loader, ANGLE_LOOP);                                                  // 切换到速度环
        DJIMotorSetRef(loader, loader->measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
@ -154,60 +191,53 @@ void ShootTask()
    // 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
    case LOAD_BURSTFIRE:
        DJIMotorOuterLoop(loader, SPEED_LOOP);
-        DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 8);
-        // x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
+        DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER);
+        // x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是)
        break;
    // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
    // 也有可能需要从switch-case中独立出来
    case LOAD_REVERSE:
-        DJIMotorOuterLoop(loader, SPEED_LOOP);
-        // ...
+        DJIMotorOuterLoop(loader, ANGLE_LOOP);                                              // 切换到角度环
+        DJIMotorSetRef(loader, loader->measure.total_angle - ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
+        hibernate_time = DWT_GetTimeline_ms();                                             // 记录触发指令的时间
+        dead_time = 300;                                                                    // 完成1发弹丸发射的时间
+        break;
        break;
    default:
        while (1)
            ; // 未知模式,停止运行,检查指针越界,内存溢出等问题
+
    }

    // 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启)
    if (shoot_cmd_recv.friction_mode == FRICTION_ON)
    {
-        // 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入
-        switch (shoot_cmd_recv.bullet_speed)
-        {
-        case SMALL_AMU_15:
-            DJIMotorSetRef(friction_l, 0);
-            DJIMotorSetRef(friction_r, 0);
-            break;
-        case SMALL_AMU_18:
-            DJIMotorSetRef(friction_l, 0);
-            DJIMotorSetRef(friction_r, 0);
-            break;
-        case SMALL_AMU_30:
-            DJIMotorSetRef(friction_l, 0);
-            DJIMotorSetRef(friction_r, 0);
-            break;
-        default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
-            DJIMotorSetRef(friction_l, 30000);
-            DJIMotorSetRef(friction_r, 30000);
-            break;
-        }
+
+            shoot_cmd_recv.motor_speed = LIMIT_MIN_MAX(shoot_cmd_recv.motor_speed,4000,8500);
+            DJIMotorSetRef(friction_r, (float)(6*shoot_cmd_recv.motor_speed));
+            DJIMotorSetRef(friction_z, (float)(6*shoot_cmd_recv.motor_speed));
+//            DJIMotorSetRef(friction_l,  (float)(6*shoot_cmd_recv.motor_speed));
    }
    else // 关闭摩擦轮
    {
-        DJIMotorSetRef(friction_l, 0);
+//        DJIMotorSetRef(friction_l, 0);
        DJIMotorSetRef(friction_r, 0);
+        DJIMotorSetRef(friction_z, 0);
    }

    // 开关弹舱盖
-    if (shoot_cmd_recv.lid_mode == LID_CLOSE)
+    if (shoot_cmd_recv.tele_mode == TELE_CLOSE)
    {
-        //...
+        Servo_Motor_FreeAngle_Set(telescope,angle);
    }
-    else if (shoot_cmd_recv.lid_mode == LID_OPEN)
+    else if (shoot_cmd_recv.tele_mode == TELE_OPEN)
    {
-        //...
+        Servo_Motor_FreeAngle_Set(telescope,60);
    }

    // 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈
+    shoot_feedback_data.Motor_Temperature = friction_r->measure.temperature;
+    //推送消息
    PubPushMessage(shoot_pub, (void *)&shoot_feedback_data);
+
 }
--- a/application/shoot/shoot.h
+++ b/application/shoot/shoot.h
@ -1,6 +1,7 @@
 #ifndef SHOOT_H
 #define SHOOT_H

+
 /**
 * @brief 发射初始化,会被RobotInit()调用
 * 
--- a/daplink.cfg
+++ b/daplink.cfg
@ -1,18 +1,34 @@
 # choose st-link/j-link/dap-link etc.
 # choose CMSIS-DAP Debugger
-adapter driver cmsis-dap
+##adapter driver dap-link
 # select SWD port
-transport select swd
+##transport select swd

 # 0x10000 = 64K Flash Size
 # 1MB on robomaster-C
-set FLASH_SIZE 0x100000
+##set FLASH_SIZE 0x100000

-source [find target/stm32f4x.cfg]
+##source [find target/stm32f4x.cfg]

 # download speed = 10MHz
 # 10MHz on FireDebugger
-adapter speed 10000
+##adapter speed 10000

 # connect under reset
-# reset_config srst_only
+# reset_config srst_only
+
+
+
+
+
+source [find interface/cmsis-dap.cfg]
+
+
+transport select swd
+
+# increase working area to 64KB
+set WORKAREASIZE 0x10000
+
+source [find target/stm32f4x.cfg]
+
+reset_config none
--- a/diary.md
+++ b/diary.md
@ -0,0 +1,3 @@
+4.20：
+改pitch电机，反馈负值相反
+电机反馈速度反馈值太陡，无法改到电机反馈
--- a/modules/algorithm/crc16.c
+++ b/modules/algorithm/crc16.c
@ -1,7 +1,29 @@
 #include "crc16.h"

 static uint8_t crc_tab16_init = 0;
-static uint16_t crc_tab16[256];
+static uint16_t crc_tab16[256] = {
+        0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48, 0x9dc1, 0xaf5a, 0xbed3,
+        0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e,
+        0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399,
+        0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5,
+        0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50,
+        0xfbef, 0xea66, 0xd8fd, 0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
+        0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285, 0x430c, 0x7197, 0x601e,
+        0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72,
+        0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5,
+        0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738,
+        0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693,
+        0xc22c, 0xd3a5, 0xe13e, 0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
+        0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1, 0x0948, 0x3bd3, 0x2a5a,
+        0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5,
+        0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710,
+        0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c,
+        0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df,
+        0x0c60, 0x1de9, 0x2f72, 0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
+        0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e, 0xf687, 0xc41c, 0xd595,
+        0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9,
+        0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c,
+        0x3de3, 0x2c6a, 0x1ef1, 0x0f78};

 /*
 * uint16_t crc_16( const unsigned char *input_str, size_t num_bytes );
@ -13,19 +35,28 @@ static uint16_t crc_tab16[256];
 */
 uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes)
 {
-    uint16_t crc;
-    const uint8_t *ptr;
-    uint16_t a;
-    if (!crc_tab16_init)
-        init_crc16_tab();
-    crc = CRC_START_16;
-    ptr = input_str;
-    if (ptr != NULL)
-        for (a = 0; a < num_bytes; a++)
-        {
-            crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t)*ptr++) & 0x00FF];
-        }
-    return crc;
+//    uint16_t crc;
+//    const uint8_t *ptr;
+//    uint16_t a;
+////    if (!crc_tab16_init)
+////        init_crc16_tab();
+//    crc = CRC_START_16;
+//    ptr = input_str;
+//    if (ptr != NULL)
+//        for (a = 0; a < num_bytes; a++) {
+//            crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t) *ptr++) & 0x00FF];
+//        }
+//    return crc;
+    uint8_t ch_data;
+    uint16_t wCRC = 0xFFFF;
+
+    if (input_str == NULL) return 0xFFFF;
+    while (num_bytes--) {
+        ch_data = *input_str++;
+        (wCRC) =
+                ((uint16_t)(wCRC) >> 8) ^ crc_tab16[((uint16_t)(wCRC) ^ (uint16_t)(ch_data)) & 0x00ff];
+    }
+    return wCRC;
 }

 /*
@ -99,3 +130,13 @@ void init_crc16_tab(void)
    }
    crc_tab16_init = 1;
 }
+uint32_t VerifyCRC16CheckSum(uint8_t *pchMessage, uint32_t dwLength)
+{
+    uint16_t wExpected = 0;
+    if ((pchMessage == NULL) || (dwLength <= 2))
+    {
+        return 0;
+    }
+    wExpected = crc_16(pchMessage, dwLength - 2);
+    return ((wExpected & 0xff) == pchMessage[dwLength - 2] && ((wExpected >> 8) & 0xff) == pchMessage[dwLength - 1]);
+}
--- a/modules/algorithm/crc16.h
+++ b/modules/algorithm/crc16.h
@ -10,5 +10,6 @@ uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes);
 uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes);
 uint16_t update_crc_16(uint16_t crc, uint8_t c);
 void init_crc16_tab(void);
+uint32_t VerifyCRC16CheckSum(uint8_t *pchMessage, uint32_t dwLength);

 #endif
--- a/modules/algorithm/user_lib.c
+++ b/modules/algorithm/user_lib.c
@ -212,3 +212,32 @@ void MatInit(mat *m, uint8_t row, uint8_t col)
    m->numRows = row;
    m->pData = (float *)zmalloc(row * col * sizeof(float));
 }
+/**
+  * @brief          一阶低通滤波初始化
+  * @author         RM
+  * @param[in]      一阶低通滤波结构体
+  * @param[in]      间隔的时间，单位 s
+  * @param[in]      滤波参数
+  * @retval         返回空
+  */
+void first_order_filter_init(first_order_filter_type_t *first_order_filter_type, float32_t frame_period, const float32_t num[1])
+{
+    first_order_filter_type->frame_period = frame_period;
+    first_order_filter_type->num[0] = num[0];
+    first_order_filter_type->input = 0.0f;
+    first_order_filter_type->out = 0.0f;
+}
+
+/**
+  * @brief          一阶低通滤波计算
+  * @author         RM
+  * @param[in]      一阶低通滤波结构体
+  * @param[in]      间隔的时间，单位 s
+  * @retval         返回空
+  */
+void first_order_filter_cali(first_order_filter_type_t *first_order_filter_type, float32_t input)
+{
+    first_order_filter_type->input = input;
+    first_order_filter_type->out =
+            first_order_filter_type->num[0] / (first_order_filter_type->num[0] + first_order_filter_type->frame_period) * first_order_filter_type->out + first_order_filter_type->frame_period / (first_order_filter_type->num[0] + first_order_filter_type->frame_period) * first_order_filter_type->input;
+}
--- a/modules/algorithm/user_lib.h
+++ b/modules/algorithm/user_lib.h
@ -32,6 +32,13 @@
 #define mcos(x) (arm_cos_f32(x))

 typedef arm_matrix_instance_f32 mat;
+typedef __packed struct
+{
+    float32_t  input;        //输入数据
+    float32_t out;          //滤波输出的数据
+    float32_t num[1];       //滤波参数
+    float32_t frame_period; //滤波的时间间隔 单位 s
+} first_order_filter_type_t;
 // 若运算速度不够,可以使用q31代替f32,但是精度会降低
 #define MatAdd arm_mat_add_f32
 #define MatSubtract arm_mat_sub_f32
@ -120,7 +127,8 @@ void Cross3d(float *v1, float *v2, float *res);
 float Dot3d(float *v1, float *v2);

 float AverageFilter(float new_data, float *buf, uint8_t len);
-
+void first_order_filter_cali(first_order_filter_type_t *first_order_filter_type, float32_t input);
+void first_order_filter_init(first_order_filter_type_t *first_order_filter_type, float32_t frame_period, const float32_t num[1]);
 #define rad_format(Ang) loop_float_constrain((Ang), -PI, PI)

 #endif
--- a/modules/auto_aim/auto_aim.c
+++ b/modules/auto_aim/auto_aim.c
@ -0,0 +1,415 @@
+//
+// Created by sph on 2024/1/21.
+//
+#include "auto_aim.h"
+#include "arm_math.h"
+
+/**
+ * @brief          选择目标装甲板（瞄装甲板）
+ * @author         SJQ
+ * @param[in]      trajectory_cal:弹道解算结构体
+ * @retval         返回空
+ */
+int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal) {
+    aim_sel->delay_time = trajectory_cal->fly_time + trajectory_cal->extra_delay_time;
+    aim_sel->target_state.yaw += aim_sel->target_state.v_yaw * aim_sel->delay_time;
+
+    //计算四块装甲板的位置
+    int use_1 = 1;
+    int i = 0;
+    int idx = -1; // 选择的装甲板
+    // 为平衡步兵
+    if (aim_sel->target_state.armor_num == 2) {
+        for (i = 0; i < 2; i++) {
+            float tmp_yaw = aim_sel->target_state.yaw + i * PI;
+            float r = aim_sel->target_state.r1;
+
+            aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * arm_cos_f32(tmp_yaw);
+            aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * arm_sin_f32(tmp_yaw);
+            aim_sel->armor_pose[i].z = aim_sel->target_state.z;
+            aim_sel->armor_pose[i].yaw = aim_sel->target_state.yaw + i * PI;
+        }
+
+        // 平衡步兵选择两块装甲板中较近的装甲板
+        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);
+        if (distance_temp < distance_min) {
+            distance_min = distance_temp;
+            idx = 1;
+        }else{
+            idx = 0;
+        }
+
+    } else if (aim_sel->target_state.armor_num == 3)//前哨站
+    {
+        for (i = 0; i < 3; i++) {
+            float tmp_yaw;
+            if (aim_sel->target_state.v_yaw <= 0){
+                tmp_yaw = aim_sel->target_state.yaw + i * (2.0f * PI / 3.0f);
+            }
+            else{
+                tmp_yaw = aim_sel->target_state.yaw - i * (2.0f * PI / 3.0f);
+            }
+            float r = use_1 ? aim_sel->target_state.r1 : aim_sel->target_state.r2;
+            aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * arm_cos_f32(tmp_yaw);
+            aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * arm_sin_f32(tmp_yaw);
+            aim_sel->armor_pose[i].z = use_1 ? aim_sel->target_state.z : aim_sel->target_state.z +
+                                                                         aim_sel->target_state.dz;
+            aim_sel->armor_pose[i].yaw = aim_sel->target_state.yaw + i * (2.0f * PI / 3.0f);
+            use_1 = !use_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])
+                label = i;
+        }
+        idx = label;
+    } else {
+        for (i = 0; i < 4; i++) {
+            float tmp_yaw = aim_sel->target_state.yaw + i * PI / 2.0f;
+            float r = use_1 ? aim_sel->target_state.r1 : aim_sel->target_state.r2;
+
+            aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * arm_cos_f32(tmp_yaw);
+            aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * arm_sin_f32(tmp_yaw);
+            aim_sel->armor_pose[i].z = use_1 ? aim_sel->target_state.z : aim_sel->target_state.z +
+                                                                         aim_sel->target_state.dz;
+            aim_sel->armor_pose[i].yaw = tmp_yaw;
+            use_1 = !use_1;
+        }
+
+//        //计算枪管到目标装甲板yaw最小的那个装甲板
+//        float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
+//        for (i = 1; i < 4; i++) {
+//            float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw);
+//            if (temp_yaw_diff < yaw_diff_min) {
+//                yaw_diff_min = temp_yaw_diff;
+//                idx = i;
+//            }
+//        }
+
+        // 选择两块较近的装甲板
+        float distance[3];
+        for (i = 0; i < 3; i++) {
+            distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
+        }
+
+        int label_first = 0;
+        int label_second = 1;
+
+        if(distance[label_first] > distance[label_second])
+        {
+            label_first = 1;
+            label_second = 0;
+        }
+
+        if(distance[2]<distance[label_second])
+            label_second = 2;
+
+
+        //再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
+        float center_length = sqrtf(powf(aim_sel->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;
+    }
+    aim_sel->aim_point[0] = aim_sel->armor_pose[idx].x + aim_sel->target_state.vx * aim_sel->delay_time;
+    aim_sel->aim_point[1] = aim_sel->armor_pose[idx].y + aim_sel->target_state.vy * aim_sel->delay_time;
+    aim_sel->aim_point[2] = aim_sel->armor_pose[idx].z + aim_sel->target_state.vz * aim_sel->delay_time;
+    return idx;
+}
+/**
+ * @brief          选择目标装甲板
+ * @author         SJQ
+ * @param[in]      trajectory_cal:弹道解算结构体
+ * @retval         返回空
+ */
+int aim_center_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal) {
+    aim_sel->delay_time = trajectory_cal->fly_time + trajectory_cal->extra_delay_time;
+    aim_sel->target_state.yaw += aim_sel->target_state.v_yaw * aim_sel->delay_time;
+
+    //计算四块装甲板的位置
+    int use_1 = 1;
+    int i = 0;
+    int idx = 0; // 选择的装甲板
+
+    float center_length = sqrtf(powf(aim_sel->target_state.x, 2) + powf(aim_sel->target_state.y, 2));
+
+    // 为平衡步兵
+    if (aim_sel->target_state.armor_num == 2) {
+        for (i = 0; i < 2; i++) {
+            float tmp_yaw = aim_sel->target_state.yaw + i * PI;
+            float r = aim_sel->target_state.r1;
+
+            aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * arm_cos_f32(tmp_yaw);
+            aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * arm_sin_f32(tmp_yaw);
+            aim_sel->armor_pose[i].z = aim_sel->target_state.z;
+            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);
+        if (distance_temp < distance_min) {
+            distance_min = distance_temp;
+            idx = 1;
+        }
+
+    } else if (aim_sel->target_state.armor_num == 3)//前哨站
+    {
+        for (i = 0; i < 3; i++) {
+            float tmp_yaw;
+            if (aim_sel->target_state.v_yaw <= 0){
+
+                tmp_yaw = aim_sel->target_state.yaw + i * (2.0f * PI / 3.0f);
+            }
+            else{
+
+                tmp_yaw = aim_sel->target_state.yaw - i * (2.0f * PI / 3.0f);
+            }
+            float r = use_1 ? aim_sel->target_state.r1 : aim_sel->target_state.r2;
+            aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * arm_cos_f32(tmp_yaw);
+            aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * arm_sin_f32(tmp_yaw);
+            aim_sel->armor_pose[i].z = use_1 ? aim_sel->target_state.z : aim_sel->target_state.z +
+                                                                         aim_sel->target_state.dz;
+            aim_sel->armor_pose[i].yaw = aim_sel->target_state.yaw + i * (2.0f * PI / 3.0f);
+            use_1 = !use_1;
+        }
+
+//        //计算枪管到目标装甲板yaw最小的那个装甲板
+//        float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
+//        for (i = 1; i < 3; i++) {
+//            float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw);
+//            if (temp_yaw_diff < yaw_diff_min) {
+//                yaw_diff_min = temp_yaw_diff;
+//                idx = i;
+//            }
+//        }
+        // 选择两块较近的装甲板，再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
+//         float distance_temp0 = powf(aim_sel->armor_pose[0].x, 2) + powf(aim_sel->armor_pose[0].y, 2);
+//         float distance_temp1 = powf(aim_sel->armor_pose[1].x, 2) + powf(aim_sel->armor_pose[1].y, 2);
+//         float distance_temp2 = powf(aim_sel->armor_pose[2].x, 2) + powf(aim_sel->armor_pose[2].y, 2);
+
+//        int label_first = 0;
+//        int label_second = 1;
+//        if (distance_temp0 > distance_temp1) {
+//            label_first = 1;
+//            if (distance_temp0 > distance_temp2) {
+//                label_second = 2;
+//            } else label_second = 0;
+//        } else {
+//            label_first = 0;
+//            if (distance_temp1 > distance_temp2) {
+//                label_second = 2;
+//            } 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])
+                label = i;
+        }
+
+        idx = label;
+    } else {
+        for (i = 0; i < 4; i++) {
+            float tmp_yaw = aim_sel->target_state.yaw + i * PI / 2.0f;
+            float r = use_1 ? aim_sel->target_state.r1 : aim_sel->target_state.r2;
+
+            aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * arm_cos_f32(tmp_yaw);
+            aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * arm_sin_f32(tmp_yaw);
+            aim_sel->armor_pose[i].z = use_1 ? aim_sel->target_state.z : aim_sel->target_state.z +
+                                                                         aim_sel->target_state.dz;
+            aim_sel->armor_pose[i].yaw = tmp_yaw;
+            use_1 = !use_1;
+        }
+
+//        //计算枪管到目标装甲板yaw最小的那个装甲板
+//        float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
+//        for (i = 1; i < 4; i++) {
+//            float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw);
+//            if (temp_yaw_diff < yaw_diff_min) {
+//                yaw_diff_min = temp_yaw_diff;
+//                idx = i;
+//            }
+//        }
+
+        // 选择两块较近de装甲板
+        // 选择两块较近的装甲板
+        float distance[3];
+        for (i = 0; i < 3; i++) {
+            distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
+        }
+
+        int label_first = 0;
+        int label_second = 1;
+
+        if(distance[label_first] > distance[label_second])
+        {
+            label_first = 1;
+            label_second = 0;
+        }
+
+        if(distance[2]<distance[label_second])
+            label_second = 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;
+    }
+
+    float r = use_1 ? aim_sel->target_state.r2 : aim_sel->target_state.r1;
+
+    aim_sel->aim_point[0] = aim_sel->target_state.x * ((center_length - r) / center_length);
+    aim_sel->aim_point[1] = aim_sel->target_state.y * ((center_length - r) / center_length);
+    aim_sel->aim_point[2] = aim_sel->target_state.z * ((center_length - r) / center_length);
+
+    return idx;
+}
+
+/**
+  * @brief          子弹飞行时间解算
+  * @author         SJQ
+  * @param[in]      x:瞄准时shooter_link下目标x坐标
+  * @param[in]      vx:瞄准时shooter_link下目标x速度
+  * @param[in]      v_x0:弹速水平分量
+  * @retval         返回空
+  */
+const float k1 = 0.015f; //标准大气压25度下空气阻力系数
+float get_fly_time(float x, float vx, float v_x0) {
+    float t = 0;
+    float f_ti = 0, df_ti = 0;
+    for (int i = 0; i < 5; i++) {
+        f_ti = logf(k1 * v_x0 * t + 1) / k1 - x - vx * t;
+        df_ti = v_x0 / (k1 * v_x0 * t + 1) - vx;
+        t = t - f_ti / df_ti;
+    }
+    return t;
+}
+
+/**
+ * @brief          解算期望云台角度（考虑子弹飞行时间）
+ * @author         SJQ
+ * @param[in]      trajectory_cal:弹道解算结构体
+ * @retval         返回空
+ */
+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.20;
+
+    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]);
+    //将目标的xyz速度转化为平行枪管与垂直枪管的速度
+    trajectory_cal->vx = trajectory_cal->velocity[0] * arm_cos_f32(trajectory_cal->alpha)
+                         + trajectory_cal->velocity[1] * arm_sin_f32(trajectory_cal->alpha);
+    trajectory_cal->vy = -trajectory_cal->velocity[0] * arm_sin_f32(trajectory_cal->alpha)
+                         + trajectory_cal->velocity[1] * arm_cos_f32(trajectory_cal->alpha);
+
+    float v_x0 = trajectory_cal->v0 * arm_cos_f32(trajectory_cal->theta_0);//水平方向弹速
+    float v_y0 = trajectory_cal->v0 * arm_sin_f32(trajectory_cal->theta_0);//竖直方向弹速
+
+    trajectory_cal->fly_time = get_fly_time(trajectory_cal->dis, trajectory_cal->vx, v_x0);
+    arm_power_f32(&trajectory_cal->fly_time, 1, &trajectory_cal->fly_time2);
+    trajectory_cal->h_r = trajectory_cal->z + trajectory_cal->velocity[2] * trajectory_cal->fly_time;
+    //开始迭代
+    trajectory_cal->theta_k = trajectory_cal->theta_0;
+    trajectory_cal->k = 0;
+    while (trajectory_cal->k < 10) {
+        v_y0 = trajectory_cal->v0 * arm_sin_f32(trajectory_cal->theta_k);//竖直方向弹速
+        trajectory_cal->h_k = v_y0 * trajectory_cal->fly_time - 0.5 * 9.8 * trajectory_cal->fly_time2;
+        trajectory_cal->err_k = trajectory_cal->h_r - trajectory_cal->h_k;
+        trajectory_cal->theta_k += 0.1 * trajectory_cal->err_k;
+        trajectory_cal->k++;
+        if (trajectory_cal->err_k < 0.005) break;
+    }
+
+    trajectory_cal->cmd_yaw = atan2f(trajectory_cal->position_xy[1],
+                                     trajectory_cal->position_xy[0]);
+
+    trajectory_cal->cmd_pitch = trajectory_cal->theta_k;
+}
+
+int auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, RecievePacket_t *receive_packet) {
+    trajectory_cal->extra_delay_time = 0.035f;//0.025
+
+    aim_sel->target_state.armor_type = receive_packet->id;
+    aim_sel->target_state.armor_num = receive_packet->armors_num;
+    aim_sel->target_state.x = receive_packet->x;
+    aim_sel->target_state.y = receive_packet->y;
+    aim_sel->target_state.z = receive_packet->z;
+    aim_sel->target_state.vx = receive_packet->vx;
+    aim_sel->target_state.vy = receive_packet->vy;
+    aim_sel->target_state.vz = receive_packet->vz;
+    aim_sel->target_state.yaw = receive_packet->yaw;
+    aim_sel->target_state.v_yaw = receive_packet->v_yaw;
+    aim_sel->target_state.r1 = receive_packet->r1;
+    aim_sel->target_state.r2 = receive_packet->r2;
+    aim_sel->target_state.dz = receive_packet->dz;
+
+    int idx = -1;
+    if(aim_sel->target_state.armor_num == 3){
+        if (aim_sel->target_state.v_yaw < -0.9){
+            aim_sel->target_state.v_yaw = -2.512;
+        }
+        else if(aim_sel->target_state.v_yaw > 0.9){
+            aim_sel->target_state.v_yaw = 2.512;
+        }
+    }
+    if(fabsf(aim_sel->target_state.v_yaw) >= 0.7* PI)
+    {
+         idx = aim_center_select(aim_sel, trajectory_cal);
+    }
+    else
+    {
+         idx = aim_armor_select(aim_sel, trajectory_cal);
+    }
+
+    trajectory_cal->position_xy[0] = aim_sel->aim_point[0];
+    trajectory_cal->position_xy[1] = aim_sel->aim_point[1];
+    trajectory_cal->z = aim_sel->aim_point[2];
+    trajectory_cal->velocity[0] = aim_sel->target_state.vx;
+    trajectory_cal->velocity[1] = aim_sel->target_state.vy;
+    trajectory_cal->velocity[2] = aim_sel->target_state.vz;
+
+    get_cmd_angle(trajectory_cal);
+    return idx;
+}
--- a/modules/auto_aim/auto_aim.h
+++ b/modules/auto_aim/auto_aim.h
@ -0,0 +1,77 @@
+//
+// Created by sph on 2024/1/21.
+//
+
+#ifndef BASIC_FRAMEWORK_AUTO_AIM_H
+#define BASIC_FRAMEWORK_AUTO_AIM_H
+
+#include "master_process.h"
+//弹道解算
+typedef  struct
+{
+    float v0;        //子弹射速
+    float velocity[3];//目标xyz速度
+    float vx;         //目标相对枪口方向的速度
+    float vy;
+    float alpha;      //目标初始航向角
+    float position_xy[2];//目标xy坐标
+    float z;         //目标z坐标
+    float fly_time;  //子弹飞行时间
+    float fly_time2;  //子弹飞行时间平方
+    float extra_delay_time ;
+    float theta_0;   //初始目标角度
+    float theta_k;   //迭代目标角度
+    float dis;       //目标距离
+    float dis2;       //目标距离平方
+    float err_k;      //迭代误差
+    uint8_t k;      //迭代次数
+    float h_k;       //迭代高度
+    float h_r;       //目标真实高度
+
+    float cmd_yaw;
+    float cmd_pitch;
+} Trajectory_Type_t;
+
+//整车状态
+typedef struct
+{
+    float x;
+    float y;
+    float z;
+    float yaw;
+    float vx;
+    float vy;
+    float vz;
+    float v_yaw;
+    float r1;
+    float r2;
+    float dz;
+    uint8_t armor_type;
+    uint8_t armor_num;
+}Target_State_Type_t;
+
+//预瞄点
+typedef struct
+{
+    float x;
+    float y;
+    float z;
+    float yaw;
+}Armor_Pose_Type_t;
+
+typedef struct
+{
+    Target_State_Type_t target_state; //整车状态
+    Armor_Pose_Type_t armor_pose[4]; //四个装甲板状态
+    float aim_point[3];                //预瞄点
+    float delay_time; //预瞄时间差
+    int suggest_fire;
+}Aim_Select_Type_t;
+
+
+int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal);
+float get_fly_time(float x, float vx, float v_x0);
+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);
+
+#endif //BASIC_FRAMEWORK_AUTO_AIM_H
--- a/modules/imu/ins_task.c
+++ b/modules/imu/ins_task.c
@ -19,6 +19,7 @@
 #include "user_lib.h"
 #include "general_def.h"
 #include "master_process.h"
+#include "crc16.h"

 static INS_t INS;
 static IMU_Param_t IMU_Param;
@ -158,8 +159,8 @@ void INS_Task(void)
        EarthFrameToBodyFrame(gravity, gravity_b, INS.q);
        for (uint8_t i = 0; i < 3; ++i) // 同样过一个低通滤波
        {
-            INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt);
-        }
+            INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) +
+                                   INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt);        }
        BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n

        INS.Yaw = QEKF_INS.Yaw;
@ -167,7 +168,8 @@ void INS_Task(void)
        INS.Roll = QEKF_INS.Roll;
        INS.YawTotalAngle = QEKF_INS.YawTotalAngle;

-        VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll);
+        //VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll);
+        VisionSetAltitude(INS.Yaw * PI / 180, INS.Pitch * PI / 180);
    }

    // temperature control
--- a/modules/master_machine/master_process.c
+++ b/modules/master_machine/master_process.c
@ -13,25 +13,36 @@
 #include "daemon.h"
 #include "bsp_log.h"
 #include "robot_def.h"
+#include "crc16.h"

-static Vision_Recv_s recv_data;
-static Vision_Send_s send_data;
+static RecievePacket_t recv_data;
+static SendPacket_t send_data;
 static DaemonInstance *vision_daemon_instance;

-void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed)
+//void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed)
+//{
+//    send_data.enemy_color = enemy_color;
+//    send_data.work_mode = work_mode;
+//    send_data.bullet_speed = bullet_speed;
+//}
+void VisionSetFlag(Enemy_Color_e enemy_color)
 {
-    send_data.enemy_color = enemy_color;
-    send_data.work_mode = work_mode;
-    send_data.bullet_speed = bullet_speed;
+    send_data.detect_color = enemy_color;
+    send_data.reserved = 0;
 }

-void VisionSetAltitude(float yaw, float pitch, float roll)
+void VisionSetAltitude(float yaw, float pitch)
 {
    send_data.yaw = yaw;
    send_data.pitch = pitch;
-    send_data.roll = roll;
 }

+void VisionSetAim(float aim_x, float aim_y, float aim_z)
+{
+    send_data.aim_x = aim_x;
+    send_data.aim_y = aim_y;
+    send_data.aim_z = aim_z;
+}
 /**
 * @brief 离线回调函数,将在daemon.c中被daemon task调用
 * @attention 由于HAL库的设计问题,串口开启DMA接收之后同时发送有概率出现__HAL_LOCK()导致的死锁,使得无法
@ -113,19 +124,26 @@ void VisionSend()
 #ifdef VISION_USE_VCP

 #include "bsp_usb.h"
-static uint8_t *vis_recv_buff;
+static uint8_t *vis_recv_buff; //接收到的原始数据

 static void DecodeVision(uint16_t recv_len)
 {
-    uint16_t flag_register;
-    get_protocol_info(vis_recv_buff, &flag_register, (uint8_t *)&recv_data.pitch);
-    // TODO: code to resolve flag_register;
+//    uint16_t flag_register;
+//    get_protocol_info(vis_recv_buff, &flag_register, (uint8_t *)&recv_data.pitch);
+//    // TODO: code to resolve flag_register;
+    if(vis_recv_buff[0]==0xA5)
+    {
+        if(VerifyCRC16CheckSum(vis_recv_buff,sizeof(recv_data)))
+        {
+            memcpy(&recv_data,vis_recv_buff,sizeof(recv_data));
+        }
+    }
 }

 /* 视觉通信初始化 */
-Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle)
+RecievePacket_t *VisionInit(void)
 {
-    UNUSED(_handle); // 仅为了消除警告
+//    UNUSED(_handle); // 仅为了消除警告
    USB_Init_Config_s conf = {.rx_cbk = DecodeVision};
    vis_recv_buff = USBInit(conf);

@ -142,14 +160,23 @@ Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle)

 void VisionSend()
 {
-    static uint16_t flag_register;
-    static uint8_t send_buff[VISION_SEND_SIZE];
-    static uint16_t tx_len;
-    // TODO: code to set flag_register
-    flag_register = 30 << 8 | 0b00000001;
-    // 将数据转化为seasky协议的数据包
-    get_protocol_send_data(0x02, flag_register, &send_data.yaw, 3, send_buff, &tx_len);
-    USBTransmit(send_buff, tx_len);
+//    static uint16_t flag_register;
+//    static uint8_t send_buff[VISION_SEND_SIZE];
+//    static uint16_t tx_len;
+//    // TODO: code to set flag_register
+//    flag_register = 30 << 8 | 0b00000001;
+//    // 将数据转化为seasky协议的数据包
+//    get_protocol_send_data(0x02, flag_register, &send_data.yaw, 3, send_buff, &tx_len);
+//    USBTransmit(send_buff, tx_len);
+    static uint8_t send_buffer[24]={0};
+
+    send_data.header = 0x5A;
+//    VisionSetFlag(1);
+    //VisionSetAim(recv_data.x,recv_data.y,recv_data.z);
+    send_data.checksum = crc_16(&send_data.header,sizeof(send_data)-2);
+
+    memcpy(send_buffer,&send_data,sizeof(send_data));
+    USBTransmit(send_buffer, sizeof(send_data));
 }

 #endif // VISION_USE_VCP
--- a/modules/master_machine/master_process.h
+++ b/modules/master_machine/master_process.h
@ -47,9 +47,11 @@ typedef struct

 typedef enum
 {
-	COLOR_NONE = 0,
-	COLOR_BLUE = 1,
-	COLOR_RED = 2,
+//	COLOR_NONE = 0,
+//	COLOR_BLUE = 1,
+//	COLOR_RED = 2,
+    ENEMY_COLOR_RED = 0,
+    ENEMY_COLOR_BLUE = 1,
 } Enemy_Color_e;

 typedef enum
@ -59,26 +61,51 @@ typedef enum
 	VISION_MODE_BIG_BUFF = 2
 } Work_Mode_e;

-typedef enum
-{
-	BULLET_SPEED_NONE = 0,
-	BIG_AMU_10 = 10,
-	SMALL_AMU_15 = 15,
-	BIG_AMU_16 = 16,
-	SMALL_AMU_18 = 18,
-	SMALL_AMU_30 = 30,
-} Bullet_Speed_e;
+

 typedef struct
 {
 	Enemy_Color_e enemy_color;
 	Work_Mode_e work_mode;
-	Bullet_Speed_e bullet_speed;

 	float yaw;
 	float pitch;
 	float roll;
 } Vision_Send_s;
+
+typedef __packed struct {
+    uint8_t header;//0x5A
+    uint8_t detect_color: 1;
+    uint8_t reserved: 7;
+    float pitch;
+    float yaw;
+    float aim_x;
+    float aim_y;
+    float aim_z;
+    uint16_t checksum;
+} SendPacket_t;
+
+typedef __packed struct 
+{
+    uint8_t header; //= 0xA5;
+    uint8_t tracking: 1;
+    uint8_t id: 3;          // 0-outpost 6-guard 7-base
+    uint8_t armors_num: 3;  // 2-balance 3-outpost 4-normal
+    uint8_t reserved: 1;
+    float x;
+    float y;
+    float z;
+    float yaw;
+    float vx;
+    float vy;
+    float vz;
+    float v_yaw;
+    float r1;
+    float r2;
+    float dz;
+    uint16_t checksum;
+} RecievePacket_t;
+
 #pragma pack()

 /**
@ -86,7 +113,8 @@ typedef struct
 *
 * @param handle 用于和视觉通信的串口handle(C板上一般为USART1,丝印为USART2,4pin)
 */
-Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle);
+//Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle);
+RecievePacket_t *VisionInit(void);

 /**
 * @brief 发送视觉数据
@ -101,14 +129,16 @@ void VisionSend();
 * @param work_mode
 * @param bullet_speed
 */
-void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed);
-
+//void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed);
+void VisionSetFlag(Enemy_Color_e enemy_color);
 /**
 * @brief 设置发送数据的姿态部分
 *
 * @param yaw
 * @param pitch
 */
-void VisionSetAltitude(float yaw, float pitch, float roll);
+//void VisionSetAltitude(float yaw, float pitch, float roll);
+void VisionSetAltitude(float yaw, float pitch);

+void VisionSetAim(float aim_x, float aim_y,float aim_z);
 #endif // !MASTER_PROCESS_H
--- a/modules/motor/DJImotor/dji_motor.c
+++ b/modules/motor/DJImotor/dji_motor.c
@ -2,6 +2,8 @@
 #include "general_def.h"
 #include "bsp_dwt.h"
 #include "bsp_log.h"
+#include "power_meter/power_meter.h"
+

 static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
 /* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
@ -29,7 +31,7 @@ static CANInstance sender_assignment[6] = {
 };

 /**
- * @brief 6个用于确认是否有电机注册到sender_assignment中的标志位,防止发送空帧,此变量将在DJIMotorControl()使用
+ * @brief 10个用于确认是否有电机注册到sender_assignment中的标志位,防止发送空帧,此变量将在DJIMotorControl()使用
 *        flag的初始化在 MotorSenderGrouping()中进行
 */
 static uint8_t sender_enable_flag[6] = {0};
@ -166,7 +168,6 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
    instance->motor_settings = config->controller_setting_init_config; // 正反转,闭环类型等

    // motor controller init 电机控制器初始化
-    PIDInit(&instance->motor_controller.current_PID, &config->controller_param_init_config.current_PID);
    PIDInit(&instance->motor_controller.speed_PID, &config->controller_param_init_config.speed_PID);
    PIDInit(&instance->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID);
    instance->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
@ -228,7 +229,17 @@ void DJIMotorSetRef(DJIMotorInstance *motor, float ref)
 {
    motor->motor_controller.pid_ref = ref;
 }
+//
+static const float motor_power_K[3] = {1.6301e-6f,5.7501e-7f,2.5863e-7f};
+//依据3508电机功率模型，预测电机输出功率
+static float EstimatePower(DJIMotorInstance* chassis_motor,float output)
+{
+    float I_cmd = chassis_motor->motor_controller.current_PID.Output;
+    float w = chassis_motor->measure.speed_aps /6 ;//aps to rpm

+    float power = motor_power_K[0] * I_cmd * w + motor_power_K[1]*w*w  + motor_power_K[2]*I_cmd*I_cmd;
+    return power;
+}
 // 为所有电机实例计算三环PID,发送控制报文
 void DJIMotorControl()
 {
@ -265,11 +276,12 @@ void DJIMotorControl()
        }

        // 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
+
+
        if ((motor_setting->close_loop_type & SPEED_LOOP) && (motor_setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
        {
            if (motor_setting->feedforward_flag & SPEED_FEEDFORWARD)
                pid_ref += *motor_controller->speed_feedforward_ptr;
-
            if (motor_setting->speed_feedback_source == OTHER_FEED)
                pid_measure = *motor_controller->other_speed_feedback_ptr;
            else // MOTOR_FEED
@ -289,6 +301,39 @@ void DJIMotorControl()
        if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
            pid_ref *= -1;

+        //功率限制
+        if(motor_setting->power_limit_flag == POWER_LIMIT_ON)
+        {
+            float I_cmd = pid_ref;
+            float w = measure->speed_aps /6 ;//aps to rpm
+            motor_controller->motor_power_predict = motor_power_K[0] * I_cmd *w + motor_power_K[1]*w*w + motor_power_K[2]*I_cmd*I_cmd+1.021f;
+            float K = motor_controller->motor_power_scale;
+            if(K>=1 || motor_controller->motor_power_predict < 0)//未超过最大功率时 或做负功的电机 不做限制
+            {
+
+            }
+            else if(K<1)
+            {
+                float P_cmd = K * motor_controller->motor_power_predict; //对输出功率进行缩放
+
+                float a = motor_power_K[2] ;
+                float b = motor_power_K[0] * w;
+                float c = motor_power_K[1] * w * w - P_cmd +1.021f;
+
+                if(pid_ref < 0)
+                {
+                    float temp = (-b - sqrtf(b*b-4*a*c))/(2*a);
+                    if(temp < -28000) pid_ref = -28000;
+                    else pid_ref = temp;
+                }
+                else
+                {
+                    float temp = (-b + sqrtf(b*b-4*a*c))/(2*a);
+                    if(temp > 28000) pid_ref = 28000;
+                    else pid_ref = temp;
+                }
+            }
+        }
        // 获取最终输出
        set = (int16_t)pid_ref;

--- a/modules/motor/DMmotor/dm4310.c
+++ b/modules/motor/DMmotor/dm4310.c
@ -0,0 +1,230 @@
+#include "dm4310.h"
+#include "memory.h"
+#include "general_def.h"
+#include "user_lib.h"
+#include "cmsis_os.h"
+#include "string.h"
+#include "daemon.h"
+#include "stdlib.h"
+#include "bsp_log.h"
+
+static uint8_t idx;
+static DMMotorInstance *dm_motor_instance[DM_MOTOR_CNT];
+static osThreadId dm_task_handle[DM_MOTOR_CNT];
+/* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */
+static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
+{
+    float span = x_max - x_min;
+    float offset = x_min;
+    return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span);
+}
+static float uint_to_float(int x_int, float x_min, float x_max, int bits)
+{
+    float span = x_max - x_min;
+    float offset = x_min;
+    return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
+}
+
+void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor)
+{
+    memset(motor->motor_can_instance->tx_buff, 0xff, 7);  // 发送电机指令的时候前面7bytes都是0xff
+    motor->motor_can_instance->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id
+    CANTransmit(motor->motor_can_instance, 1);
+}
+
+static void DMMotorDecode(CANInstance *motor_can)
+{
+    uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
+    uint8_t *rxbuff = motor_can->rx_buff;
+    DMMotorInstance *motor = (DMMotorInstance *)motor_can->id;
+    DM_Motor_Measure_s *measure = &(motor->measure); // 将can实例中保存的id转换成电机实例的指针
+
+    DaemonReload(motor->motor_daemon);
+
+    measure->last_position = measure->position;
+
+    measure->state = (uint8_t)(rxbuff[0]>>4);
+
+    tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
+    measure->position = uint_to_float(tmp, DM_P_MIN, DM_P_MAX, 16);
+
+    measure->angle_single_round =  RAD_2_DEGREE * (measure->position+3.141593);
+
+    tmp = (uint16_t)((rxbuff[3] << 4) | rxbuff[4] >> 4);
+    measure->velocity = uint_to_float(tmp, DM_V_MIN, DM_V_MAX, 12);
+
+    tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
+    measure->torque = uint_to_float(tmp, DM_T_MIN, DM_T_MAX, 12);
+
+    measure->T_Mos = (float)rxbuff[6];
+    measure->T_Rotor = (float)rxbuff[7];
+
+    // 多圈角度计算,前提是假设两次采样间电机转过的角度小于180°,自己画个图就清楚计算过程了
+    if (measure->position - measure->last_position > 3.1425926)
+        measure->total_round--;
+    else if (measure->position - measure->last_position < -3.1415926)
+        measure->total_round++;
+
+    measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
+}
+
+static void DMMotorLostCallback(void *motor_ptr)
+{
+    DMMotorInstance *motor = (DMMotorInstance *)motor_ptr;
+    uint16_t can_bus = motor->motor_can_instance->can_handle == &hcan1 ? 1 : 2;
+    LOGWARNING("[dm_motor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instance->tx_id);
+}
+void DMMotorCaliEncoder(DMMotorInstance *motor)
+{
+    DMMotorSetMode(DM_CMD_ZERO_POSITION, motor);
+    DWT_Delay(0.1f);
+}
+DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config)
+{
+    DMMotorInstance *motor = (DMMotorInstance *)malloc(sizeof(DMMotorInstance));
+    memset(motor, 0, sizeof(DMMotorInstance));
+
+    config->can_init_config.rx_id = config->can_init_config.rx_id;
+    config->can_init_config.tx_id = config->can_init_config.tx_id;
+    motor->motor_settings = config->controller_setting_init_config;
+    PIDInit(&motor->motor_controller.current_PID, &config->controller_param_init_config.current_PID);
+    PIDInit(&motor->motor_controller.speed_PID, &config->controller_param_init_config.speed_PID);
+    PIDInit(&motor->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID);
+    motor->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
+    motor->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
+
+    config->can_init_config.can_module_callback = DMMotorDecode;
+    config->can_init_config.id = motor;
+    motor->motor_can_instance = CANRegister(&config->can_init_config);
+
+    Daemon_Init_Config_s conf = {
+            .callback = DMMotorLostCallback,
+            .owner_id = motor,
+            .reload_count = 10,
+    };
+    motor->motor_daemon = DaemonRegister(&conf);
+
+    DMMotorEnable(motor);
+    DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
+    DWT_Delay(0.1f);
+    dm_motor_instance[idx++] = motor;
+    return motor;
+}
+
+void DMMotorSetRef(DMMotorInstance *motor, float ref)
+{
+    motor->motor_controller.pid_ref = ref;
+}
+
+void DMMotorEnable(DMMotorInstance *motor)
+{
+    motor->stop_flag = MOTOR_ENALBED;
+}
+
+void DMMotorStop(DMMotorInstance *motor)//不使用使能模式是因为需要收到反馈
+{
+    motor->stop_flag = MOTOR_STOP;
+}
+
+void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type)
+{
+    motor->motor_settings.outer_loop_type = type;
+}
+
+
+//还得使用力矩控制
+void DMMotorTask(void const *argument)
+{
+    float  pid_ref, set, pid_measure;
+
+    DMMotorInstance *motor = (DMMotorInstance *)argument;
+    Motor_Controller_s *motor_controller;   // 电机控制器
+    DM_Motor_Measure_s *measure = &motor->measure;
+    motor_controller = &motor->motor_controller;
+    Motor_Control_Setting_s *setting = &motor->motor_settings;
+    //CANInstance *motor_can = motor->motor_can_instance;
+    //uint16_t tmp;
+    DMMotor_Send_s motor_send_mailbox;
+    while (1)
+    {
+        pid_ref = motor->motor_controller.pid_ref;//保存设定值
+
+        if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
+            pid_ref *= -1;
+
+        // pid_ref会顺次通过被启用的闭环充当数据的载体
+        // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
+        if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP)
+        {
+            if (setting->angle_feedback_source == OTHER_FEED)
+                pid_measure = *motor_controller->other_angle_feedback_ptr;
+            else
+                pid_measure = measure->total_angle; // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃
+            // 更新pid_ref进入下一个环
+            pid_ref = PIDCalculate(&motor_controller->angle_PID, pid_measure, pid_ref);
+        }
+
+        // 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
+
+
+        if ((setting->close_loop_type & SPEED_LOOP) && (setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
+        {
+            if (setting->feedforward_flag & SPEED_FEEDFORWARD)
+                pid_ref += *motor_controller->speed_feedforward_ptr;
+            if (setting->speed_feedback_source == OTHER_FEED)
+                pid_measure = *motor_controller->other_speed_feedback_ptr;
+            else // MOTOR_FEED
+                pid_measure = measure->velocity;
+            // 更新pid_ref进入下一个环
+            pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref);
+        }
+
+        if (setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
+            pid_ref *= -1;
+
+        set = pid_ref;
+
+        if (motor->stop_flag == MOTOR_STOP)
+           set = 0;
+
+        LIMIT_MIN_MAX(set, DM_T_MIN, DM_T_MAX);
+        motor_send_mailbox.position_des = float_to_uint(0, DM_P_MIN, DM_P_MAX, 16);
+        motor_send_mailbox.velocity_des = float_to_uint(0, DM_V_MIN, DM_V_MAX, 12);
+        motor_send_mailbox.torque_des = float_to_uint(set, DM_T_MIN, DM_T_MAX, 12);
+        motor_send_mailbox.Kp = 0;//只使用力矩控制无需kpkd参数
+        motor_send_mailbox.Kd = 0;//只使用力矩控制无需kpkd参数
+
+
+
+        motor->motor_can_instance->tx_buff[0] = (uint8_t)(motor_send_mailbox.position_des >> 8);
+        motor->motor_can_instance->tx_buff[1] = (uint8_t)(motor_send_mailbox.position_des);
+        motor->motor_can_instance->tx_buff[2] = (uint8_t)(motor_send_mailbox.velocity_des >> 4);
+        motor->motor_can_instance->tx_buff[3] = (uint8_t)(((motor_send_mailbox.velocity_des & 0xF) << 4) | (motor_send_mailbox.Kp >> 8));
+        motor->motor_can_instance->tx_buff[4] = (uint8_t)(motor_send_mailbox.Kp);
+        motor->motor_can_instance->tx_buff[5] = (uint8_t)(motor_send_mailbox.Kd >> 4);
+        motor->motor_can_instance->tx_buff[6] = (uint8_t)(((motor_send_mailbox.Kd & 0xF) << 4) | (motor_send_mailbox.torque_des >> 8));
+        motor->motor_can_instance->tx_buff[7] = (uint8_t)(motor_send_mailbox.torque_des);
+
+
+
+
+        CANTransmit(motor->motor_can_instance, 1);
+
+        osDelay(2);
+    }
+}
+void DMMotorControlInit()
+{
+    char dm_task_name[5] = "dm";
+    // 遍历所有电机实例,创建任务
+    if (!idx)
+        return;
+    for (size_t i = 0; i < idx; i++)
+    {
+        char dm_id_buff[2] = {0};
+        __itoa(i, dm_id_buff, 10);
+        strcat(dm_task_name, dm_id_buff);
+        osThreadDef(dm_task_name, DMMotorTask, osPriorityNormal, 0, 128);
+        dm_task_handle[i] = osThreadCreate(osThread(dm_task_name), dm_motor_instance[i]);
+    }
+}
--- a/modules/motor/DMmotor/dm4310.h
+++ b/modules/motor/DMmotor/dm4310.h
@ -0,0 +1,75 @@
+#ifndef DM4310_H
+#define DM4310_H
+#include <stdint.h>
+#include "bsp_can.h"
+#include "controller.h"
+#include "motor_def.h"
+#include "daemon.h"
+
+#define DM_MOTOR_CNT 1
+
+#define DM_P_MIN  (-3.1415926f)
+#define DM_P_MAX  3.1415926f
+#define DM_V_MIN  (-45.0f)
+#define DM_V_MAX  45.0f
+#define DM_T_MIN  (-18.0f)
+#define DM_T_MAX   18.0f
+
+typedef struct
+{
+    uint8_t id;
+    uint8_t state;
+    float velocity;     //速度
+    float last_position;    //上次位置
+    float position; //位置
+    float torque;   //力矩
+    float T_Mos;    //mos温度
+    float T_Rotor; //电机温度
+    float angle_single_round; //单圈角度
+    int32_t total_round; //总圈数
+    float total_angle; //总角度
+}DM_Motor_Measure_s;
+
+typedef struct
+{
+    uint16_t position_des;
+    uint16_t velocity_des;
+    uint16_t torque_des;
+    uint16_t Kp;
+    uint16_t Kd;
+}DMMotor_Send_s;
+typedef struct
+{
+    DM_Motor_Measure_s measure;
+    Motor_Control_Setting_s motor_settings;
+    Motor_Controller_s motor_controller;//电机控制器
+
+    CANInstance *motor_can_instance;//电机can实例
+
+    Motor_Type_e motor_type;//电机类型
+    Motor_Working_Type_e stop_flag;
+
+    DaemonInstance* motor_daemon;
+    uint32_t lost_cnt;
+}DMMotorInstance;
+
+typedef enum
+{
+    DM_CMD_MOTOR_MODE = 0xfc,   // 使能,会响应指令
+    DM_CMD_RESET_MODE = 0xfd,   // 停止
+    DM_CMD_ZERO_POSITION = 0xfe, // 将当前的位置设置为编码器零位
+    DM_CMD_CLEAR_ERROR = 0xfb // 清除电机过热错误
+}DMMotor_Mode_e;
+
+DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config);
+
+void DMMotorSetRef(DMMotorInstance *motor, float ref);
+
+void DMMotorOuterLoop(DMMotorInstance *motor,Closeloop_Type_e closeloop_type);
+
+void DMMotorEnable(DMMotorInstance *motor);
+void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor);
+void DMMotorStop(DMMotorInstance *motor);
+void DMMotorCaliEncoder(DMMotorInstance *motor);
+void DMMotorControlInit();
+#endif // !DMMOTOR
--- a/modules/motor/HTmotor/HT04.c
+++ b/modules/motor/HTmotor/HT04.c
@ -116,6 +116,7 @@ HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config)

    config->can_init_config.can_module_callback = HTMotorDecode;
    config->can_init_config.id = motor;
+    config->can_init_config.rx_id = config->can_init_config.tx_id;
    motor->motor_can_instace = CANRegister(&config->can_init_config);

    Daemon_Init_Config_s conf = {
--- a/modules/motor/motor_def.h
+++ b/modules/motor/motor_def.h
@ -67,7 +67,11 @@ typedef enum
    MOTOR_STOP = 0,
    MOTOR_ENALBED = 1,
 } Motor_Working_Type_e;
-
+typedef enum
+{
+    NO_POWER_LIMIT = 0,
+    POWER_LIMIT_ON = 1,
+} Power_Limit_Type_e;
 /* 电机控制设置,包括闭环类型,反转标志和反馈来源 */
 typedef struct
 {
@ -78,7 +82,7 @@ typedef struct
    Feedback_Source_e angle_feedback_source;       // 角度反馈类型
    Feedback_Source_e speed_feedback_source;       // 速度反馈类型
    Feedfoward_Type_e feedforward_flag;            // 前馈标志
-
+    Power_Limit_Type_e power_limit_flag;            //功率限制标志
 } Motor_Control_Setting_s;

 /* 电机控制器,包括其他来源的反馈数据指针,3环控制器和电机的参考输入*/
@ -95,12 +99,17 @@ typedef struct
    PIDInstance angle_PID;

    float pid_ref; // 将会作为每个环的输入和输出顺次通过串级闭环
+
+    float motor_power_max;     //每个电机分配的功率上限
+    float motor_power_predict; //根据模型预测的电机功率
+    float motor_power_scale;   //电机功率缩放比例
 } Motor_Controller_s;

 /* 电机类型枚举 */
 typedef enum
 {
    MOTOR_TYPE_NONE = 0,
+    DM4310,
    GM6020,
    M3508,
    M2006,
--- a/modules/motor/motor_task.c
+++ b/modules/motor/motor_task.c
@ -13,7 +13,7 @@ void MotorControlTask()
    DJIMotorControl();

    /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */
-    LKMotorControl();
+//    LKMotorControl();

    // legacy support
    // 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞
--- a/modules/power_meter/power_meter.c
+++ b/modules/power_meter/power_meter.c
@ -0,0 +1,38 @@
+//
+// Created by SJQ on 2023/12/19.
+//
+
+#include "power_meter.h"
+#include "memory.h"
+#include "stdlib.h"
+
+static PowerMeterInstance *power_meter_instance = NULL; // 可以由app保存此指针
+
+static void PowerMeterRxCallback(CANInstance *_instance)
+{
+    uint8_t *rxbuff;
+    PowerMeter_Msg_s *Msg;
+    rxbuff = _instance->rx_buff;
+    Msg = &power_meter_instance->power_msg;
+    Msg->vol = (uint16_t)(rxbuff[1] << 8 | rxbuff[0]);
+    Msg->current = (uint16_t)(rxbuff[3] << 8 | rxbuff[2]);
+
+}
+
+PowerMeterInstance *PowerMeterInit(PowerMeter_Init_Config_s *PowerMeter_config)
+{
+    power_meter_instance = (PowerMeterInstance *)malloc(sizeof(PowerMeterInstance));
+    memset(power_meter_instance, 0, sizeof(PowerMeterInstance));
+
+    PowerMeter_config->can_config.can_module_callback = PowerMeterRxCallback;
+    power_meter_instance->can_ins = CANRegister(&PowerMeter_config->can_config);
+    return power_meter_instance;
+}
+
+float PowerMeterGet(PowerMeterInstance *instance)
+{
+    float power = instance->power_msg.current*instance->power_msg.vol/1e4;
+    return power;
+}
+
+#include "power_meter.h"
--- a/modules/power_meter/power_meter.h
+++ b/modules/power_meter/power_meter.h
@ -0,0 +1,39 @@
+//
+// Created by SJQ on 2023/12/19.
+//
+
+#ifndef BASIC_FRAMEWORK_POWER_METER_H
+#define BASIC_FRAMEWORK_POWER_METER_H
+#include "bsp_can.h"
+
+#pragma pack(1)
+typedef struct
+{
+    uint16_t vol; // 电压
+    uint16_t current; // 电流
+} PowerMeter_Msg_s;
+#pragma pack()
+
+/* 超级电容实例 */
+typedef struct
+{
+    CANInstance *can_ins; // CAN实例
+    PowerMeter_Msg_s power_msg; // 功率计反馈信息
+} PowerMeterInstance;
+
+/* 超级电容初始化配置 */
+typedef struct
+{
+    CAN_Init_Config_s can_config;
+} PowerMeter_Init_Config_s;
+
+/**
+ * @brief 初始化功率计
+ *
+ * @param PowerMeter_config 功率计初始化配置
+ * @return PowerMeterInstance* 功率计实例指针
+ */
+PowerMeterInstance *PowerMeterInit(PowerMeter_Init_Config_s *PowerMeter_config);
+float PowerMeterGet(PowerMeterInstance *instance);
+
+#endif //BASIC_FRAMEWORK_POWER_METER_H
--- a/modules/referee/referee_protocol.h
+++ b/modules/referee/referee_protocol.h
@ -83,24 +83,28 @@ typedef enum
 	ID_robot_hurt = 0x0206,				   // 伤害状态数据
 	ID_shoot_data = 0x0207,				   // 实时射击数据
 	ID_student_interactive = 0x0301,	   // 机器人间交互数据
+    ID_custom_robot = 0x302,               // 自定义控制器数据（图传链路）
+    ID_remote_control = 0x304,             // 键鼠遥控数据（图传链路）
 } CmdID_e;

 /* 命令码数据段长,根据官方协议来定义长度，还有自定义数据长度 */
 typedef enum
 {
-	LEN_game_state = 3,							 // 0x0001
+	LEN_game_state = 11,							 // 0x0001
 	LEN_game_result = 1,						 // 0x0002
-	LEN_game_robot_HP = 2,						 // 0x0003
+	LEN_game_robot_HP = 32,						 // 0x0003
 	LEN_event_data = 4,							 // 0x0101
 	LEN_supply_projectile_action = 4,			 // 0x0102
-	LEN_game_robot_state = 27,					 // 0x0201
-	LEN_power_heat_data = 14,					 // 0x0202
+	LEN_game_robot_state = 13,					 // 0x0201
+	LEN_power_heat_data = 16,					 // 0x0202
 	LEN_game_robot_pos = 16,					 // 0x0203
-	LEN_buff_musk = 1,							 // 0x0204
-	LEN_aerial_robot_energy = 1,				 // 0x0205
+	LEN_buff_musk = 6,							 // 0x0204
+	LEN_aerial_robot_energy = 2,				 // 0x0205
 	LEN_robot_hurt = 1,							 // 0x0206
 	LEN_shoot_data = 7,							 // 0x0207
 	LEN_receive_data = 6 + Communicate_Data_LEN, // 0x0301
+    LEN_self_control = 15,                     // 0x0303
+    LEN_remote_control = 12,                     // 0x0304

 } JudgeDataLength_e;

@ -157,37 +161,32 @@ typedef struct
 	uint8_t supply_projectile_num;
 } ext_supply_projectile_action_t;

-/* ID: 0X0201  Byte: 27    机器人状态数据 */
+/* ID: 0X0201  Byte: 27    V1.6.1 机器人状态数据 */
 typedef struct
 {
 	uint8_t robot_id;
 	uint8_t robot_level;
-	uint16_t remain_HP;
-	uint16_t max_HP;
-	uint16_t shooter_id1_17mm_cooling_rate;
-	uint16_t shooter_id1_17mm_cooling_limit;
-	uint16_t shooter_id1_17mm_speed_limit;
-	uint16_t shooter_id2_17mm_cooling_rate;
-	uint16_t shooter_id2_17mm_cooling_limit;
-	uint16_t shooter_id2_17mm_speed_limit;
-	uint16_t shooter_id1_42mm_cooling_rate;
-	uint16_t shooter_id1_42mm_cooling_limit;
-	uint16_t shooter_id1_42mm_speed_limit;
+	uint16_t current_HP;
+	uint16_t maximum_HP;
+	uint16_t shooter_barrel_cooling_value;
+    uint16_t shooter_barrel_heat_limit;
 	uint16_t chassis_power_limit;
+
 	uint8_t mains_power_gimbal_output : 1;
 	uint8_t mains_power_chassis_output : 1;
 	uint8_t mains_power_shooter_output : 1;
 } ext_game_robot_state_t;

-/* ID: 0X0202  Byte: 14    实时功率热量数据 */
+/* ID: 0X0202  Byte: 14    V1.6.1实时功率热量数据 */
 typedef struct
 {
-	uint16_t chassis_volt;
+	uint16_t chassis_voltage;
 	uint16_t chassis_current;
 	float chassis_power;		   // 瞬时功率
-	uint16_t chassis_power_buffer; // 60焦耳缓冲能量
-	uint16_t shooter_heat0;		   // 17mm
-	uint16_t shooter_heat1;
+	uint16_t buffer_energy; // 60焦耳缓冲能量
+    uint16_t shooter_17mm_1_barrel_heat;
+    uint16_t shooter_17mm_2_barrel_heat;
+    uint16_t shooter_42mm_barrel_heat;
 } ext_power_heat_data_t;

 /* ID: 0x0203  Byte: 16    机器人位置数据 */
@ -227,6 +226,20 @@ typedef struct
 	float bullet_speed;
 } ext_shoot_data_t;

+/****************************图传链路数据****************************/
+/* ID: 0x0304  Byte:  7    图传链路键鼠遥控数据 */
+typedef struct
+{
+    int16_t mouse_x;
+    int16_t mouse_y;
+    int16_t mouse_z;
+    int8_t left_button_down;
+    int8_t right_button_down;
+    uint16_t keyboard_value;
+    uint16_t reserved;
+}vision_transfer_t;
+/****************************图传链路数据****************************/
+
 /****************************机器人交互数据****************************/
 /****************************机器人交互数据****************************/
 /* 发送的内容数据段最大为 113 检测是否超出大小限制?实际上图形段不会超，数据段最多30个，也不会超*/
--- a/modules/referee/referee_task.c
+++ b/modules/referee/referee_task.c
@ -14,7 +14,7 @@
 #include "referee_UI.h"
 #include "string.h"
 #include "cmsis_os.h"
-
+Referee_Interactive_info_t ui_data;
 static Referee_Interactive_info_t *Interactive_data; // UI绘制需要的机器人状态数据
 static referee_info_t *referee_recv_info;            // 接收到的裁判系统数据
 uint8_t UI_Seq;                                      // 包序号，供整个referee文件使用
@ -28,7 +28,7 @@ uint8_t UI_Seq;                                      // 包序号，供整个ref
 */
 static void DeterminRobotID()
 {
-    // id小于7是红色,大于7是蓝色,0为红色，1为蓝色   #define Robot_Red 0    #define Robot_Blue 1
+    // id小于7是红色,大于7是蓝色   #define Robot_Red 0    #define Robot_Blue 1
    referee_recv_info->referee_id.Robot_Color = referee_recv_info->GameRobotState.robot_id > 7 ? Robot_Blue : Robot_Red;
    referee_recv_info->referee_id.Robot_ID = referee_recv_info->GameRobotState.robot_id;
    referee_recv_info->referee_id.Cilent_ID = 0x0100 + referee_recv_info->referee_id.Robot_ID; // 计算客户端ID
@ -49,33 +49,44 @@ referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Int

 void UITask()
 {
-    RobotModeTest(Interactive_data); // 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
+//    RobotModeTest(Interactive_data); // 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
    MyUIRefresh(referee_recv_info, Interactive_data);
+
 }

-static Graph_Data_t UI_shoot_line[10]; // 射击准线
+static Graph_Data_t UI_shoot_line[11]; // 射击准线
+static Graph_Data_t UI_shoot_circle[1]; //自瞄标识
 static Graph_Data_t UI_Energy[3];      // 电容能量条
 static String_Data_t UI_State_sta[6];  // 机器人状态,静态只需画一次
 static String_Data_t UI_State_dyn[6];  // 机器人状态,动态先add才能change
-static uint32_t shoot_line_location[10] = {540, 960, 490, 515, 565};
+static uint32_t shoot_line_location[10] = {540, 960, 490, 515, 565,940,1110,465,440};

 void MyUIInit()
 {
-    if (!referee_recv_info->init_flag)
-        vTaskDelete(NULL); // 如果没有初始化裁判系统则直接删除ui任务
+//    if (!referee_recv_info->init_flag)
+//        vTaskDelete(NULL); // 如果没有初始化裁判系统则直接删除ui任务
    while (referee_recv_info->GameRobotState.robot_id == 0)
        osDelay(100); // 若还未收到裁判系统数据,等待一段时间后再检查

    DeterminRobotID();                                            // 确定ui要发送到的目标客户端
    UIDelete(&referee_recv_info->referee_id, UI_Data_Del_ALL, 0); // 清空UI

+
    // 绘制发射基准线
-    UILineDraw(&UI_shoot_line[0], "sl0", UI_Graph_ADD, 7, UI_Color_White, 3, 710, shoot_line_location[0], 1210, shoot_line_location[0]);
-    UILineDraw(&UI_shoot_line[1], "sl1", UI_Graph_ADD, 7, UI_Color_White, 3, shoot_line_location[1], 340, shoot_line_location[1], 740);
-    UILineDraw(&UI_shoot_line[2], "sl2", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[2], 1110, shoot_line_location[2]);
-    UILineDraw(&UI_shoot_line[3], "sl3", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[3], 1110, shoot_line_location[3]);
-    UILineDraw(&UI_shoot_line[4], "sl4", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[4], 1110, shoot_line_location[4]);
-    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_line[4]);
+    UILineDraw(&UI_shoot_line[0], "sl0", UI_Graph_ADD, 7, UI_Color_White, 1, 710, shoot_line_location[0], 1210, shoot_line_location[0]);
+    UILineDraw(&UI_shoot_line[1], "sl1", UI_Graph_ADD, 7, UI_Color_White, 1, 740, shoot_line_location[1], 340, shoot_line_location[1]);
+    UILineDraw(&UI_shoot_line[2], "sl2", UI_Graph_ADD, 7, UI_Color_White, 1, 810, shoot_line_location[2], 1110, shoot_line_location[2]);
+    UILineDraw(&UI_shoot_line[3], "sl3", UI_Graph_ADD, 7, UI_Color_Yellow, 1, 810, shoot_line_location[3], 1110, shoot_line_location[3]);
+    UILineDraw(&UI_shoot_line[4], "sl4", UI_Graph_ADD, 7, UI_Color_Yellow, 1, 810, shoot_line_location[4], 1110, shoot_line_location[4]);
+    UILineDraw(&UI_shoot_line[5], "sl5", UI_Graph_ADD, 7, UI_Color_Yellow, 1, shoot_line_location[5], 200, shoot_line_location[5], 800);
+    UIFloatDraw(&UI_shoot_line[6], "sl6", UI_Graph_ADD, 7, UI_Color_Green, 18, 2, 2, 650, 400, 5500000);
+    UIGraphRefresh(&referee_recv_info->referee_id, 7, UI_shoot_line[0], UI_shoot_line[1], UI_shoot_line[2], UI_shoot_line[3], UI_shoot_line[4], UI_shoot_line[5],UI_shoot_line[6]);
+
+    UIFloatDraw(&UI_shoot_line[7], "sl7", UI_Graph_ADD, 7, UI_Color_Green, 18, 2, 2, 850, 400, 26000);
+    UILineDraw(&UI_shoot_line[8], "sl8", UI_Graph_ADD, 7, UI_Color_Yellow, 1, shoot_line_location[6], 200, shoot_line_location[6], 800);
+    UILineDraw(&UI_shoot_line[9], "sl9", UI_Graph_ADD, 7, UI_Color_Yellow, 1, 810, shoot_line_location[7], 1110, shoot_line_location[7]);
+    UILineDraw(&UI_shoot_line[10], "sl10", UI_Graph_ADD, 7, UI_Color_White, 1, 810, shoot_line_location[8], 1110, shoot_line_location[8]);
+    UIGraphRefresh(&referee_recv_info->referee_id,5, UI_shoot_line[7],UI_shoot_line[8],UI_shoot_line[9],UI_shoot_line[10]);

    // 绘制车辆状态标志指示
    UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 150, 750, "chassis:");
@ -85,34 +96,36 @@ void MyUIInit()
    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]);
    UICharDraw(&UI_State_sta[3], "ss3", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 150, 600, "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, "lid:");
-    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[4]);
+

    // 绘制车辆状态标志，动态
    // 由于初始化时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, 15, 2, 270, 750, "follow");
    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, "free");
    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");
    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");
    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 ");
-    UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]);
+
+//    UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 270, 550, "open ");
+//    UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]);

    // 底盘功率显示，静态
-    UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 7, UI_Color_Green, 18, 2, 620, 230, "Power:");
+    UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 620, 230, "Power_MAX:");
    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[5]);
    // 能量条框
    UIRectangleDraw(&UI_Energy[0], "ss6", UI_Graph_ADD, 7, UI_Color_Green, 2, 720, 140, 1220, 180);
    UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[0]);

    // 底盘功率显示,动态
-    UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 2, 750, 230, 24000);
+    UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 2, 850, 230, referee_recv_info->GameRobotState.chassis_power_limit*1000);
    // 能量条初始状态
    UILineDraw(&UI_Energy[2], "sd6", UI_Graph_ADD, 8, UI_Color_Pink, 30, 720, 160, 1020, 160);
+
    UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]);
 }

@ -135,7 +148,7 @@ static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) // 测
        _Interactive_data->gimbal_mode = GIMBAL_ZERO_FORCE;
        _Interactive_data->shoot_mode = SHOOT_ON;
        _Interactive_data->friction_mode = FRICTION_ON;
-        _Interactive_data->lid_mode = LID_OPEN;
+        _Interactive_data->tele_mode = TELE_OPEN;
        _Interactive_data->Chassis_Power_Data.chassis_power_mx += 3.5;
        if (_Interactive_data->Chassis_Power_Data.chassis_power_mx >= 18)
            _Interactive_data->Chassis_Power_Data.chassis_power_mx = 0;
@ -147,7 +160,7 @@ static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) // 测
        _Interactive_data->gimbal_mode = GIMBAL_FREE_MODE;
        _Interactive_data->shoot_mode = SHOOT_OFF;
        _Interactive_data->friction_mode = FRICTION_OFF;
-        _Interactive_data->lid_mode = LID_CLOSE;
+        _Interactive_data->tele_mode = TELE_CLOSE;
        break;
    }
    case 2:
@ -156,7 +169,7 @@ static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) // 测
        _Interactive_data->gimbal_mode = GIMBAL_GYRO_MODE;
        _Interactive_data->shoot_mode = SHOOT_ON;
        _Interactive_data->friction_mode = FRICTION_ON;
-        _Interactive_data->lid_mode = LID_OPEN;
+        _Interactive_data->tele_mode = TELE_OPEN;
        break;
    }
    case 3:
@ -165,7 +178,7 @@ static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) // 测
        _Interactive_data->gimbal_mode = GIMBAL_ZERO_FORCE;
        _Interactive_data->shoot_mode = SHOOT_OFF;
        _Interactive_data->friction_mode = FRICTION_OFF;
-        _Interactive_data->lid_mode = LID_CLOSE;
+        _Interactive_data->tele_mode = TELE_CLOSE;
        break;
    }
    default:
@ -237,20 +250,60 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i
        _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");
-        UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]);
-        _Interactive_data->Referee_Interactive_Flag.lid_flag = 0;
-    }
+//    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");
+//        UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]);
+//        _Interactive_data->Referee_Interactive_Flag.lid_flag = 0;
+//    }
    // power
    if (_Interactive_data->Referee_Interactive_Flag.Power_flag == 1)
    {
-        UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_Change, 8, UI_Color_Green, 18, 2, 2, 750, 230, _Interactive_data->Chassis_Power_Data.chassis_power_mx * 1000);
-        UILineDraw(&UI_Energy[2], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 30, 720, 160, (uint32_t)750 + _Interactive_data->Chassis_Power_Data.chassis_power_mx * 30, 160);
-        UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]);
+        UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_Change, 8, UI_Color_Green, 18, 2, 2, 850, 230, _Interactive_data->Chassis_Power_Data.chassis_power_mx * 1000);
+        UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[1]);
        _Interactive_data->Referee_Interactive_Flag.Power_flag = 0;
    }
+    //Cap
+    if (_Interactive_data->Referee_Interactive_Flag.Cap_flag == 1)
+    {
+        UILineDraw(&UI_Energy[2], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 30, 720, 160, 720 + (_Interactive_data->Chassis_Power_Data.cap_vol-12) * 42, 160);
+        UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[2]);
+        _Interactive_data->Referee_Interactive_Flag.Cap_flag = 0;
+    }
+    //motor_speed
+    if (_Interactive_data->Referee_Interactive_Flag.MotorSpeed_flag == 1)
+    {
+        UIFloatDraw(&UI_shoot_line[6], "sl6", UI_Graph_Change, 7, UI_Color_Green, 18, 2, 2, 650, 400, _Interactive_data->Chassis_Power_Data.motor_speed * 1000);
+        UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_shoot_line[6]);
+        _Interactive_data->Referee_Interactive_Flag.MotorSpeed_flag = 0;
+    }
+    //Motor_Temperature
+    if (_Interactive_data->Referee_Interactive_Flag.Temperature_flag == 1)
+    {
+        UIFloatDraw(&UI_shoot_line[7], "sl7", UI_Graph_Change, 7, UI_Color_Green, 18, 2, 2, 850, 440, _Interactive_data->Motor_Temperature*1000);
+        UIGraphRefresh(&referee_recv_info->referee_id,1, UI_shoot_line[7]);
+        _Interactive_data->Referee_Interactive_Flag.Temperature_flag = 0;
+    }
+
+    //line
+    if (_Interactive_data->Referee_Interactive_Flag.Vision_flag == 1)
+    {
+
+        switch (_Interactive_data->Vision_fire) {
+            case 0 :
+                UICircleDraw(&UI_shoot_circle[0],"sc0",UI_Graph_Del,6,UI_Color_Green,2,960,540,20);
+                UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_shoot_circle[0]);
+                break;
+            case 1 :
+                UICircleDraw(&UI_shoot_circle[0],"sc0",UI_Graph_ADD,6,UI_Color_Green,2,960,540,20);
+                UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_shoot_circle[0]);
+                break;
+
+        }
+//        UILineDraw(&UI_shoot_line[4], "sl4", UI_Graph_Change, 7, _Interactive_data->Vision_fire == 0 ? UI_Color_Yellow : UI_Color_Purplish_red, 2, 810, shoot_line_location[4], 1110, shoot_line_location[4]);
+//        UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_shoot_line[4]);
+//        _Interactive_data->Referee_Interactive_Flag.Vision_flag = 0;
+    }
 }

 /**
@ -285,10 +338,10 @@ static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data)
        _Interactive_data->friction_last_mode = _Interactive_data->friction_mode;
    }

-    if (_Interactive_data->lid_mode != _Interactive_data->lid_last_mode)
+    if (_Interactive_data->tele_mode != _Interactive_data->tele_last_mode)
    {
        _Interactive_data->Referee_Interactive_Flag.lid_flag = 1;
-        _Interactive_data->lid_last_mode = _Interactive_data->lid_mode;
+        _Interactive_data->tele_last_mode = _Interactive_data->tele_mode;
    }

    if (_Interactive_data->Chassis_Power_Data.chassis_power_mx != _Interactive_data->Chassis_last_Power_Data.chassis_power_mx)
@ -296,4 +349,24 @@ static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data)
        _Interactive_data->Referee_Interactive_Flag.Power_flag = 1;
        _Interactive_data->Chassis_last_Power_Data.chassis_power_mx = _Interactive_data->Chassis_Power_Data.chassis_power_mx;
    }
+    if (_Interactive_data->Chassis_Power_Data.cap_vol != _Interactive_data->Chassis_last_Power_Data.cap_vol)
+    {
+        _Interactive_data->Referee_Interactive_Flag.Cap_flag = 1;
+        _Interactive_data->Chassis_last_Power_Data.cap_vol = _Interactive_data->Chassis_Power_Data.cap_vol;
+    }
+    if (_Interactive_data->Vision_fire != _Interactive_data->last_Vision_fire)
+    {
+        _Interactive_data->Referee_Interactive_Flag.Vision_flag = 1;
+        _Interactive_data->last_Vision_fire = _Interactive_data->Vision_fire;
+    }
+    if (_Interactive_data->Chassis_Power_Data.motor_speed != _Interactive_data->Chassis_last_Power_Data.motor_speed)
+    {
+        _Interactive_data->Referee_Interactive_Flag.MotorSpeed_flag = 1;
+        _Interactive_data->Chassis_last_Power_Data.motor_speed = _Interactive_data->Chassis_Power_Data.motor_speed;
+    }
+    if (_Interactive_data->Motor_Temperature != _Interactive_data->Last_Motor_Temperature)
+    {
+        _Interactive_data->Referee_Interactive_Flag.Temperature_flag = 1;
+        _Interactive_data->Last_Motor_Temperature = _Interactive_data->Motor_Temperature;
+    }
 }
--- a/modules/referee/referee_task.h
+++ b/modules/referee/referee_task.h
@ -8,6 +8,7 @@
 * @brief 初始化裁判系统交互任务(UI和多机通信)
 *
 */
+extern Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI
 referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Interactive_info_t *UI_data);

 /**
--- a/modules/referee/rm_referee.h
+++ b/modules/referee/rm_referee.h
@ -38,7 +38,8 @@ typedef struct
 	ext_robot_hurt_t RobotHurt;							   // 0x0206
 	ext_shoot_data_t ShootData;							   // 0x0207

-	// 自定义交互数据的接收
+    vision_transfer_t  VisionTransfer;
+    // 自定义交互数据的接收
 	Communicate_ReceiveData_t ReceiveData;

 	uint8_t init_flag;
@ -54,6 +55,10 @@ typedef struct
 	uint32_t lid_flag : 1;
 	uint32_t friction_flag : 1;
 	uint32_t Power_flag : 1;
+    uint32_t Cap_flag : 1;
+    uint32_t Vision_flag :1;
+    uint32_t MotorSpeed_flag:1;
+    uint32_t Temperature_flag:1;
 } Referee_Interactive_Flag_t;

 // 此结构体包含UI绘制与机器人车间通信的需要的其他非裁判系统数据
@ -65,16 +70,21 @@ typedef struct
 	gimbal_mode_e gimbal_mode;				 // 云台模式
 	shoot_mode_e shoot_mode;				 // 发射模式设置
 	friction_mode_e friction_mode;			 // 摩擦轮关闭
-	lid_mode_e lid_mode;					 // 弹舱盖打开
+	tele_mode_e tele_mode;					 // 倍镜打开关闭
 	Chassis_Power_Data_s Chassis_Power_Data; // 功率控制
+    uint8_t Vision_fire;
+    uint8_t Motor_Temperature;
+

 	// 上一次的模式，用于flag判断
 	chassis_mode_e chassis_last_mode;
 	gimbal_mode_e gimbal_last_mode;
 	shoot_mode_e shoot_last_mode;
 	friction_mode_e friction_last_mode;
-	lid_mode_e lid_last_mode;
+	tele_mode_e tele_last_mode;
 	Chassis_Power_Data_s Chassis_last_Power_Data;
+    uint8_t last_Vision_fire;
+    uint8_t Last_Motor_Temperature;

 } Referee_Interactive_info_t;

--- a/modules/referee/vision_transfer.c
+++ b/modules/referee/vision_transfer.c
@ -0,0 +1,162 @@
+/**
+ * @file rm_referee.C
+ * @author kidneygood (you@domain.com)
+ * @brief
+ * @version 0.1
+ * @date 2022-11-18
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+
+#include "vision_transfer.h"
+#include "string.h"
+#include "crc_ref.h"
+#include "bsp_usart.h"
+#include "task.h"
+#include "daemon.h"
+#include "bsp_log.h"
+#include "cmsis_os.h"
+#include "remote_control.h"
+
+#define RE_RX_BUFFER_SIZE 128u // 裁判系统接收缓冲区大小
+
+static USARTInstance *vt_usart_instance; // 裁判系统串口实例
+static DaemonInstance *vision_transfer_daemon;		  // 裁判系统守护进程
+static referee_info_vt_t referee_info_vt;			  // 裁判系统数据
+static VT_ctrl_t vt_ctrl[2];     //[0]:当前数据TEMP,[1]:上一次的数据LAST.用于按键持续按下和切换的判断
+/**
+ * @brief  读取裁判数据,中断中读取保证速度
+ * @param  buff: 读取到的裁判系统原始数据
+ * @retval 是否对正误判断做处理
+ * @attention  在此判断帧头和CRC校验,无误再写入数据，不重复判断帧头
+ */
+static void JudgeReadVTData(uint8_t *buff)
+{
+    uint16_t judge_length; // 统计一帧数据长度
+    if (buff == NULL)	   // 空数据包，则不作任何处理
+        return;
+
+    // 写入帧头数据(5-byte),用于判断是否开始存储裁判数据
+    memcpy(&referee_info_vt.FrameHeader, buff, LEN_HEADER);
+
+    // 判断帧头数据(0)是否为0xA5
+    if (buff[SOF] == REFEREE_SOF)
+    {
+        // 帧头CRC8校验
+        if (Verify_CRC8_Check_Sum(buff, LEN_HEADER) == TRUE)
+        {
+            // 统计一帧数据长度(byte),用于CR16校验
+            judge_length = buff[DATA_LENGTH] + LEN_HEADER + LEN_CMDID + LEN_TAIL;
+            // 帧尾CRC16校验
+            if (Verify_CRC16_Check_Sum(buff, judge_length) == TRUE)
+            {
+                // 2个8位拼成16位int
+                referee_info_vt.CmdID = (buff[6] << 8 | buff[5]);
+                // 解析数据命令码,将数据拷贝到相应结构体中(注意拷贝数据的长度)
+                // 第8个字节开始才是数据 data=7
+                switch (referee_info_vt.CmdID)
+                {
+                    case  ID_custom_robot: //0x0302
+                        memcpy(&referee_info_vt.ReceiveData, (buff + DATA_Offset), LEN_self_control);
+                        break;
+                    case  ID_remote_control: //0x0304
+                        memcpy(&referee_info_vt.VisionTransfer, (buff + DATA_Offset), LEN_remote_control);
+                        break;
+                }
+            }
+        }
+        // 首地址加帧长度,指向CRC16下一字节,用来判断是否为0xA5,从而判断一个数据包是否有多帧数据
+        if (*(buff + sizeof(xFrameHeader) + LEN_CMDID + referee_info_vt.FrameHeader.DataLength + LEN_TAIL) == 0xA5)
+        { // 如果一个数据包出现了多帧数据,则再次调用解析函数,直到所有数据包解析完毕
+            JudgeReadVTData(buff + sizeof(xFrameHeader) + LEN_CMDID + referee_info_vt.FrameHeader.DataLength + LEN_TAIL);
+        }
+    }
+}
+static void vt_to_rc(void)
+{
+    // 鼠标解析
+    vt_ctrl[TEMP].mouse.x = referee_info_vt.VisionTransfer.mouse_x; //!< Mouse X axis
+    vt_ctrl[TEMP].mouse.y = referee_info_vt.VisionTransfer.mouse_y; //!< Mouse Y axis
+    vt_ctrl[TEMP].mouse.press_l = referee_info_vt.VisionTransfer.left_button_down;                 //!< Mouse Left Is Press ?
+    vt_ctrl[TEMP].mouse.press_r = referee_info_vt.VisionTransfer.right_button_down;                //!< Mouse Right Is Press ?
+
+//  位域的按键值解算,直接memcpy即可,注意小端低字节在前,即lsb在第一位,msb在最后
+    *(uint16_t *)&vt_ctrl[TEMP].key[KEY_PRESS] = referee_info_vt.VisionTransfer.keyboard_value;
+
+    if (vt_ctrl[TEMP].key[KEY_PRESS].ctrl) // ctrl键按下
+        vt_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL] = vt_ctrl[TEMP].key[KEY_PRESS];
+    else
+        memset(&vt_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL], 0, sizeof(Key_t));
+    if (vt_ctrl[TEMP].key[KEY_PRESS].shift) // shift键按下
+        vt_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT] = vt_ctrl[TEMP].key[KEY_PRESS];
+    else
+        memset(&vt_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT], 0, sizeof(Key_t));
+
+    uint16_t key_now = vt_ctrl[TEMP].key[KEY_PRESS].keys,                   // 当前按键是否按下
+    key_last = vt_ctrl[LAST].key[KEY_PRESS].keys,                       // 上一次按键是否按下
+    key_with_ctrl = vt_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL].keys,        // 当前ctrl组合键是否按下
+    key_with_shift = vt_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT].keys,      //  当前shift组合键是否按下
+    key_last_with_ctrl = vt_ctrl[LAST].key[KEY_PRESS_WITH_CTRL].keys,   // 上一次ctrl组合键是否按下
+    key_last_with_shift = vt_ctrl[LAST].key[KEY_PRESS_WITH_SHIFT].keys; // 上一次shift组合键是否按下
+
+    for (uint16_t i = 0, j = 0x1; i < 16; j <<= 1, i++)
+    {
+        if (i == 4 || i == 5) // 4,5位为ctrl和shift,直接跳过
+            continue;
+// 如果当前按键按下,上一次按键没有按下,且ctrl和shift组合键没有按下,则按键按下计数加1(检测到上升沿)
+        if ((key_now & j) && !(key_last & j) && !(key_with_ctrl & j) && !(key_with_shift & j))
+            vt_ctrl[TEMP].key_count[KEY_PRESS][i]++;
+// 当前ctrl组合键按下,上一次ctrl组合键没有按下,则ctrl组合键按下计数加1(检测到上升沿)
+        if ((key_with_ctrl & j) && !(key_last_with_ctrl & j))
+            vt_ctrl[TEMP].key_count[KEY_PRESS_WITH_CTRL][i]++;
+// 当前shift组合键按下,上一次shift组合键没有按下,则shift组合键按下计数加1(检测到上升沿)
+        if ((key_with_shift & j) && !(key_last_with_shift & j))
+            vt_ctrl[TEMP].key_count[KEY_PRESS_WITH_SHIFT][i]++;
+    }
+
+    memcpy(&vt_ctrl[LAST], &vt_ctrl[TEMP], sizeof(VT_ctrl_t)); // 保存上一次的数据,用于按键持续按下和切换的判断
+}
+
+/*裁判系统串口接收回调函数,解析数据 */
+static void VTRefereeRxCallback()
+{
+    DaemonReload(vision_transfer_daemon);
+    JudgeReadVTData(vt_usart_instance->recv_buff);
+    vt_to_rc();
+}
+// 裁判系统丢失回调函数,重新初始化裁判系统串口
+static void VTRefereeLostCallback(void *arg)
+{
+    USARTServiceInit(vt_usart_instance);
+    LOGWARNING("[rm_ref] lost referee vision data ");
+}
+
+/* 裁判系统通信初始化 */
+VT_ctrl_t *VTRefereeInit(UART_HandleTypeDef *vt_usart_handle)
+{
+    USART_Init_Config_s conf;
+    conf.module_callback = VTRefereeRxCallback;
+    conf.usart_handle = vt_usart_handle;
+    conf.recv_buff_size = RE_RX_BUFFER_SIZE; // mx 255(u8)
+    vt_usart_instance = USARTRegister(&conf);
+
+    Daemon_Init_Config_s daemon_conf = {
+            .callback = VTRefereeLostCallback,
+            .owner_id = vt_usart_instance,
+            .reload_count = 30, // 0.3s没有收到数据,则认为丢失,重启串口接收
+    };
+    vision_transfer_daemon = DaemonRegister(&daemon_conf);
+
+    return &vt_ctrl;
+}
+
+/**
+ * @brief 裁判系统数据发送函数
+ * @param
+ */
+void VTRefereeSend(uint8_t *send, uint16_t tx_len)
+{
+    USARTSend(vt_usart_instance, send, tx_len, USART_TRANSFER_DMA);
+    osDelay(115);
+}
--- a/modules/referee/vision_transfer.h
+++ b/modules/referee/vision_transfer.h
@ -0,0 +1,46 @@
+#ifndef VISION_TRANSFER_H
+#define VISION_TRANSFER_H
+
+#include "usart.h"
+#include "referee_protocol.h"
+#include "robot_def.h"
+#include "bsp_usart.h"
+#include "FreeRTOS.h"
+#include "remote_control.h"
+
+#pragma pack(1)
+
+// 此结构体包含裁判系统接收数据以及UI绘制与机器人车间通信的相关信息
+typedef struct
+{
+
+	xFrameHeader FrameHeader; // 接收到的帧头信息
+	uint16_t CmdID;
+
+    vision_transfer_t  VisionTransfer;
+	Communicate_ReceiveData_t ReceiveData;
+
+	uint8_t init_flag;
+
+} referee_info_vt_t;
+
+#pragma pack()
+
+/**
+ * @brief 裁判系统通信初始化,该函数会初始化裁判系统串口,开启中断
+ *
+ * @param vt_usart_handle 串口handle,C板一般用串口6
+ * @return referee_info_t* 返回裁判系统反馈的数据,包括热量/血量/状态等
+ */
+VT_ctrl_t *VTRefereeInit(UART_HandleTypeDef *referee_usart_handle);
+
+/**
+ * @brief UI绘制和交互数的发送接口,由UI绘制任务和多机通信函数调用
+ * @note 内部包含了一个实时系统的延时函数,这是因为裁判系统接收CMD数据至高位10Hz
+ *
+ * @param send 发送数据首地址
+ * @param tx_len 发送长度
+ */
+void VTRefereeSend(uint8_t *send, uint16_t tx_len);
+
+#endif // !REFEREE_H
--- a/modules/remote/remote_control.c
+++ b/modules/remote/remote_control.c
@ -5,15 +5,21 @@
 #include "stdlib.h"
 #include "daemon.h"
 #include "bsp_log.h"
+#include "referee_protocol.h"
+#include "rm_referee.h"

 #define REMOTE_CONTROL_FRAME_SIZE 18u // 遥控器接收的buffer大小

 // 遥控器数据
 static RC_ctrl_t rc_ctrl[2];     //[0]:当前数据TEMP,[1]:上一次的数据LAST.用于按键持续按下和切换的判断
+
+
 static uint8_t rc_init_flag = 0; // 遥控器初始化标志位

 // 遥控器拥有的串口实例,因为遥控器是单例,所以这里只有一个,就不封装了
+// 第二个串口实例，有可能出现问题
 static USARTInstance *rc_usart_instance;
+
 static DaemonInstance *rc_daemon_instance;

 /**
@ -103,6 +109,7 @@ static void RemoteControlRxCallback()
 */
 static void RCLostCallback(void *id)
 {
+    DaemonReload(rc_daemon_instance);         // 遥控器没数据也可以喂狗
    memset(rc_ctrl, 0, sizeof(rc_ctrl)); // 清空遥控器数据
    USARTServiceInit(rc_usart_instance); // 尝试重新启动接收
    LOGWARNING("[rc] remote control lost");
@ -133,4 +140,4 @@ uint8_t RemoteControlIsOnline()
    if (rc_init_flag)
        return DaemonIsOnline(rc_daemon_instance);
    return 0;
-}
+}
--- a/modules/remote/remote_control.h
+++ b/modules/remote/remote_control.h
@ -113,6 +113,20 @@ typedef struct
    uint8_t key_count[3][16];
 } RC_ctrl_t;

+typedef struct
+{
+    struct
+    {
+        int16_t x;
+        int16_t y;
+        uint8_t press_l;
+        uint8_t press_r;
+    } mouse;
+
+    Key_t key[3];
+    uint8_t key_count[3][16];
+}VT_ctrl_t; //图传链路下发的遥控数据
+
 /* ------------------------- Internal Data ----------------------------------- */

 /**
@ -123,6 +137,7 @@ typedef struct
 */
 RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle);

+
 /**
 * @brief 检查遥控器是否在线,若尚未初始化也视为离线
 *
--- a/modules/super_cap/super_cap.c
+++ b/modules/super_cap/super_cap.c
@ -1,14 +1,8 @@
-/*
- * @Descripttion:
- * @version:
- * @Author: Chenfu
- * @Date: 2022-12-02 21:32:47
- * @LastEditTime: 2022-12-05 15:29:49
- */
 #include "super_cap.h"
 #include "memory.h"
 #include "stdlib.h"

+
 static SuperCapInstance *super_cap_instance = NULL; // 可以由app保存此指针

 static void SuperCapRxCallback(CANInstance *_instance)
@ -17,18 +11,20 @@ static void SuperCapRxCallback(CANInstance *_instance)
    SuperCap_Msg_s *Msg;
    rxbuff = _instance->rx_buff;
    Msg = &super_cap_instance->cap_msg;
-    Msg->vol = (uint16_t)(rxbuff[0] << 8 | rxbuff[1]);
-    Msg->current = (uint16_t)(rxbuff[2] << 8 | rxbuff[3]);
-    Msg->power = (uint16_t)(rxbuff[4] << 8 | rxbuff[5]);
+    Msg->input_vol = (int16_t)(rxbuff[1] << 8 | rxbuff[0]);
+    Msg->cap_vol = (int16_t)(rxbuff[3] << 8 | rxbuff[2]);
+    Msg->input_cur = (int16_t)(rxbuff[5] << 8 | rxbuff[4]);
+    Msg->power_set = (int16_t)(rxbuff[7] << 8 | rxbuff[6]);
 }

 SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config)
 {
    super_cap_instance = (SuperCapInstance *)malloc(sizeof(SuperCapInstance));
    memset(super_cap_instance, 0, sizeof(SuperCapInstance));
-    
+
    supercap_config->can_config.can_module_callback = SuperCapRxCallback;
    super_cap_instance->can_ins = CANRegister(&supercap_config->can_config);
+    PIDInit(&super_cap_instance->buffer_pid, &supercap_config->buffer_pid_config);
    return super_cap_instance;
 }

@ -38,6 +34,15 @@ void SuperCapSend(SuperCapInstance *instance, uint8_t *data)
    CANTransmit(instance->can_ins,1);
 }

+void SuperCapSetPower(SuperCapInstance *instance, float power_set)
+{
+    uint16_t tmpPower = (uint16_t)(power_set * 100);
+    uint8_t data[8] = {0};
+    data[0] = tmpPower >> 8;
+    data[1] = tmpPower;
+    SuperCapSend(instance,data);
+}
+
 SuperCap_Msg_s SuperCapGet(SuperCapInstance *instance)
 {
    return instance->cap_msg;
--- a/modules/super_cap/super_cap.h
+++ b/modules/super_cap/super_cap.h
@ -1,21 +1,16 @@
-/*
- * @Descripttion:
- * @version:
- * @Author: Chenfu
- * @Date: 2022-12-02 21:32:47
- * @LastEditTime: 2022-12-05 15:25:46
- */
 #ifndef SUPER_CAP_H
 #define SUPER_CAP_H

 #include "bsp_can.h"
+#include "controller.h"

 #pragma pack(1)
 typedef struct
 {
-    uint16_t vol; // 电压
-    uint16_t current; // 电流
-    uint16_t power; // 功率
+    int16_t input_vol; // 输入电压
+    int16_t cap_vol;   // 电容电压
+    int16_t input_cur; // 输入电流
+    int16_t power_set; // 设定功率
 } SuperCap_Msg_s;
 #pragma pack()

@ -24,17 +19,19 @@ typedef struct
 {
    CANInstance *can_ins; // CAN实例
    SuperCap_Msg_s cap_msg; // 超级电容信息
+    PIDInstance buffer_pid; //缓冲功率闭环控制参数
 } SuperCapInstance;

 /* 超级电容初始化配置 */
 typedef struct
 {
    CAN_Init_Config_s can_config;
+    PID_Init_Config_s buffer_pid_config;
 } SuperCap_Init_Config_s;

 /**
 * @brief 初始化超级电容
- * 
+ *
 * @param supercap_config 超级电容初始化配置
 * @return SuperCapInstance* 超级电容实例指针
 */
@ -42,10 +39,18 @@ SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config);

 /**
 * @brief 发送超级电容控制信息
- * 
+ *
 * @param instance 超级电容实例
 * @param data 超级电容控制信息
 */
 void SuperCapSend(SuperCapInstance *instance, uint8_t *data);

+/**
+ * @brief 发送超级电容目标功率
+ *
+ * @param instance 超级电容实例
+ * @param power_set 超级电容目标功率
+ */
+void SuperCapSetPower(SuperCapInstance *instance, float power_set);
+
 #endif // !SUPER_CAP_Hd
Author	SHA1	Message	Date
zcj	589d260cf3	更新UI	2024-05-17 06:23:21 +08:00
zcj	89609e884a	功率控制加超电，参数可以更激进一点	2024-05-16 22:15:26 +08:00
zcj	7825e6e55f	更新裁判系统串口命令码数据段长度，解决缓冲功率接收错误的问题	2024-05-16 17:55:25 +08:00
zcj	cbfb2990d2	pitch限位改成联盟赛版本，电机角角度计算不准，会出问题	2024-05-16 15:30:08 +08:00
zcj	a3e0cb1a87	修改炮台模式相关代码，机械改头，调整一下	2024-05-16 01:53:47 +08:00
shmily744	6ca069c064	自瞄选板修改	2024-05-14 02:34:46 +08:00
zcj	5ef2702d27	倍镜电机换成舵机，自瞄待修改	2024-05-13 15:41:06 +08:00
zcj	d45d68e076	增加shift使用超电，UI显示摩擦轮转速（可调）和温度	2024-05-11 13:00:25 +08:00
zcj	713a9e1a4d	超电上车成功，pitch限位写好，侧向发射	2024-05-05 10:58:32 +08:00
zcj	c17f3f6cb7	DM4310上车测试成功，图传链路测试成功	2024-05-01 18:29:39 +08:00
zcj	28234a395f	增加达妙驱动,测试可用，需上车调试	2024-04-18 17:10:53 +08:00
zcj	0eaf1bd9d8	增加达妙驱动（未验证）	2024-04-17 15:26:00 +08:00
zcj	b0efcd64ff	联盟赛版本	2024-04-03 01:12:50 +08:00
zcj	4871c573fb	改自瞄	2024-03-27 01:09:09 +08:00
zcj	8de2e62164	修改训练赛的问题	2024-03-27 01:07:52 +08:00
zcj	b2687cafe3	修改训练赛的问题	2024-03-26 22:50:17 +08:00
zcj	2b402788d1	自瞄上操作端并优化，pitch修正失败，ui刷新完成	2024-03-23 07:44:19 +08:00
zcj	507cdf96cb	微调yaw	2024-03-19 19:47:07 +08:00
zcj	202afc84bb	细化UI，回退pitch限位为不可上坡的	2024-03-17 20:22:07 +08:00
zcj	7a85fdf4c2	UI完成	2024-03-14 15:41:33 +08:00
sph	2aac4702b9	英雄火控完成待测试	2024-03-09 23:13:39 +08:00
userName	2ee9f9e6d5	重新优化拨弹和底盘PID	2024-03-09 22:06:29 +08:00
userName	e4b8e54574	小陀螺后的功率控制	2024-03-03 15:52:51 +08:00
userName	03081a7075	pitch轴新的限位，三摩擦轮，功率控制	2024-03-02 20:43:43 +08:00
userName	fb08102e35	云台出现大问题，会失效；	2024-02-29 20:39:18 +08:00
userName	35ad6c7055	功率控制（需要调参数）、拨弹反转（未验证）	2024-02-04 10:59:41 +08:00
周楚杰	8be769bf3f	自瞄测试、云台底盘PID粗调	2024-02-01 21:43:57 +08:00
周楚杰	26e53fa916	底盘跟随，5V激光，拨弹轮	2024-01-27 10:41:36 +08:00
周楚杰	7fc13a67e6	底盘跟随，5V激光，拨弹轮	2024-01-26 17:25:52 +08:00
周楚杰	c2388e74e5	PITCH云台，小陀螺	2024-01-16 19:51:18 +08:00
周楚杰	0bf1ba7f3e	底盘代码逻辑调整完成，未调参数	2023-11-19 18:19:37 +08:00
周楚杰	29c6b56489	完成PID参数的调试	2023-10-19 21:08:43 +08:00
周楚杰	0154d1164a	完成英雄YAW和PITCH轴的简单调试，PID参数需要重新调试	2023-10-19 20:33:23 +08:00