更新UI

.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">

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/RGB modules/standard_cmd
+        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/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor modules/motor/ECmotor modules/motor/DMmotor
+        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}")

Inc/main.h

@@ -31,8 +31,7 @@ extern "C" {
 
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
-#include "robot.h"
-#include "bsp_log.h"
+
 /* USER CODE END Includes */
 
 /* Exported types ------------------------------------------------------------*/

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

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

Src/main.c

@@ -35,7 +35,8 @@
 
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
-
+#include "robot.h"
+#include "bsp_log.h"
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/

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;

application/chassis/chassis.c

@@ -22,17 +22,14 @@
 #include "bsp_dwt.h"
 #include "referee_UI.h"
 #include "arm_math.h"
+#include "power_meter/power_meter.h"
 #include "user_lib.h"
 
-#include "vofa.h"
-
 /* 根据robot_def.h中的macro自动计算的参数 */
 #define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f)     // 半轴距
 #define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f)   // 半轮距
 #define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI) // 轮子周长
 
-#define P_MAX 50.0f//功率控制 单位：W
-
 /* 底盘应用包含的模块和信息存储,底盘是单例模式,因此不需要为底盘建立单独的结构体 */
 #ifdef CHASSIS_BOARD // 如果是底盘板,使用板载IMU获取底盘转动角速度
 #include "can_comm.h"
@@ -46,14 +43,15 @@ static Subscriber_t *chassis_sub;                   // 用于订阅底盘的控
 #endif                                              // !ONE_BOARD
 static Chassis_Ctrl_Cmd_s chassis_cmd_recv;         // 底盘接收到的控制命令
 static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据
-static Chassis_Ctrl_Cmd_s Chassis_Power_Mx;
-const static float CHASSIS_ACCEL_X_NUM=0.1f;
-const static float CHASSIS_ACCEL_Y_NUM=0.1f;
 
-// 超级电容
-SuperCapInstance *cap;    // 超级电容全局
-static uint16_t last_chassis_power_limit=0;//超级电容更新
+
+
+
+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;
@@ -61,82 +59,84 @@ 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; // 底盘速度解算后的临时输出,待进行限幅
-first_order_filter_type_t vx_filter;
-first_order_filter_type_t vy_filter;
-static float P_cmd;
+float P_cmdall = 0;
+static float input=0;
+void ChassisInit()
+{
 
 
-void ChassisInit() {
     // 四个轮子的参数一样,改tx_id和反转标志位即可
     Motor_Init_Config_s chassis_motor_config = {
-            .can_init_config.can_handle = &hcan1,
-            .controller_param_init_config = {
-                    .speed_PID = {
-                            .Kp = 5.0f,
-                            .Ki = 0.01f,
-                            .Kd = 0.002f,
-                            .IntegralLimit = 3000,
-                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
-                            .MaxOut = 30000,
-                    },
-                    .current_PID = {
-                            .Kp = 0.0f,
-                            .Ki = 0.0f,
-                            .Kd = 0.0f,
-                            .IntegralLimit = 3000,
-                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
-                            .MaxOut = 15000,
-                    },
+        .can_init_config.can_handle = &hcan1,
+        .controller_param_init_config = {
+            .speed_PID = {
+                .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,
             },
-            .controller_setting_init_config = {
-                    .angle_feedback_source = MOTOR_FEED,
-                    .speed_feedback_source = MOTOR_FEED,
-                    .outer_loop_type = SPEED_LOOP,
-                    .close_loop_type = SPEED_LOOP, // CURRENT_LOOP,
-                    .power_limit_flag = POWER_LIMIT_ON,
+            .current_PID = {
+                .Kp = 0.4f, // 0.4
+                .Ki = 0.1f,   // 0
+                .Kd = 0,
+                .IntegralLimit = 3000,
+                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
+                .MaxOut = 15000,
             },
-            .motor_type = M3508,
+        },
+        .controller_setting_init_config = {
+            .angle_feedback_source = MOTOR_FEED,
+            .speed_feedback_source = MOTOR_FEED,
+            .outer_loop_type = SPEED_LOOP,
+            .close_loop_type = SPEED_LOOP,
+
+            .power_limit_flag = POWER_LIMIT_ON,
+        },
+        .motor_type = M3508,
     };
     //  @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
-    // 四个轮子pid分开
-    //右前
     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.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);
 
     chassis_motor_config.can_init_config.tx_id = 4;
-
     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 = 1;
-    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);
 
-    //超级电容
+
     SuperCap_Init_Config_s cap_conf = {
             .can_config = {
                     .can_handle = &hcan1,
                     .tx_id = 0x210,
                     .rx_id = 0x211,
-
             },
-            .buffer_config_pid = {
-                    .Kp = 1.0f,
+            .buffer_pid_config = {
+                    .Kp = 0.7f,
                     .Ki = 0,
                     .Kd = 0,
                     .MaxOut = 300,
-            },
+            }
     };
     cap = SuperCapInit(&cap_conf); // 超级电容初始化
+    SuperCapSetPower(cap,70);  // 超级电容限制功率
 
-    //用一阶滤波代替斜波函数生成                                     //增大更能刹住
-    first_order_filter_init(&vx_filter, 0.007f, &CHASSIS_ACCEL_X_NUM);
-    first_order_filter_init(&vy_filter, 0.007f, &CHASSIS_ACCEL_Y_NUM);
+//    PowerMeter_Init_Config_s power_conf = {
+//            .can_config = {
+//                    .can_handle = &hcan2,
+//                    .rx_id = 0x211,
+//            }
+//    };
+//    power_meter = PowerMeterInit(&power_conf);
 
     // 发布订阅初始化,如果为双板,则需要can comm来传递消息
 #ifdef CHASSIS_BOARD
@@ -159,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)
@@ -168,64 +168,44 @@ void ChassisInit() {
  * @brief 计算每个轮毂电机的输出,正运动学解算
  *        用宏进行预替换减小开销,运动解算具体过程参考教程
  */
-//全向轮解算
-static void OmniCalculate() {
-    vt_rf = HALF_WHEEL_BASE * chassis_cmd_recv.wz + chassis_vx * 0.707f + chassis_vy * 0.707f;
-    vt_rb = HALF_WHEEL_BASE * chassis_cmd_recv.wz + chassis_vx * 0.707f - chassis_vy * 0.707f;
-    vt_lb = HALF_WHEEL_BASE * chassis_cmd_recv.wz - chassis_vx * 0.707f - chassis_vy * 0.707f;
-    vt_lf = HALF_WHEEL_BASE * chassis_cmd_recv.wz - chassis_vx * 0.707f + chassis_vy * 0.707f;
-
-    vt_rf = (vt_rf / RADIUS_WHEEL) * 180 / PI * REDUCTION_RATIO_WHEEL;
-    vt_rb = (vt_rb / RADIUS_WHEEL) * 180 / PI * REDUCTION_RATIO_WHEEL;
-    vt_lb = (vt_lb / RADIUS_WHEEL) * 180 / PI * REDUCTION_RATIO_WHEEL;
-    vt_lf = (vt_lf / RADIUS_WHEEL) * 180 / PI * REDUCTION_RATIO_WHEEL;
-}
-
-static const float motor_power_K[3] = {1.6301e-6f,5.7501e-7f,2.5863e-7f};
-float input;
-
-float P_max = 0;
-
-///依据3508电机功率模型，预测电机输出功率
-static float EstimatePower(DJIMotorInstance* chassis_motor)
+static void MecanumCalculate()
 {
-
-    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;
+    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()
 {
-//    float Plimit = 1.0f;
-    P_cmd = 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 + 3.6f;
-
-//    if(chassis_cmd_recv.buffer_energy<50&&chassis_cmd_recv.buffer_energy>=40)	    Plimit=0.9f;
-//    else if(chassis_cmd_recv.buffer_energy<40&&chassis_cmd_recv.buffer_energy>=35)	Plimit=0.75f;
-//    else if(chassis_cmd_recv.buffer_energy<35&&chassis_cmd_recv.buffer_energy>=30)	Plimit=0.5f;
-//    else if(chassis_cmd_recv.buffer_energy<30&&chassis_cmd_recv.buffer_energy>=20)	Plimit=0.25f;
-//    else if(chassis_cmd_recv.buffer_energy<20&&chassis_cmd_recv.buffer_energy>=10)	Plimit=0.125f;
-//    else if(chassis_cmd_recv.buffer_energy<10&&chassis_cmd_recv.buffer_energy>=0)	Plimit=0.05f;
-//    else if(chassis_cmd_recv.buffer_energy==60)					                    Plimit=1.0f;
+    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.buffer_supercap ;
+         P_max = input + chassis_cmd_recv.P_SuperCap ;
     }
     else
     {
         P_max = input;
     }
-    float K = P_max / P_cmd;
+    float K = P_max/P_cmdall;
+
 
     motor_rf->motor_controller.motor_power_scale = K;
     motor_rb->motor_controller.motor_power_scale = K;
@@ -239,35 +219,43 @@ static void LimitChassisOutput()
         DJIMotorSetRef(motor_rb, vt_rb);
     }
 
+
+
 }
 /**
- * @brief 超电功率对缓冲功率进行闭环
- *
+ * @brief 每次随等级更新超电的设定功率
  *
  */
 static void SuperCapSetUpdate()
 {
 
-    PIDCalculate(&cap->buffer_pid, chassis_cmd_recv.buffer_energy,15);//对缓冲功率进行闭环
+    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的数据
  *
  */
-static void EstimateSpeed() {
+static void EstimateSpeed()
+{
     // 根据电机速度和陀螺仪的角速度进行解算,还可以利用加速度计判断是否打滑(如果有)
     // chassis_feedback_data.vx vy wz =
     //  ...
 }
-static chassis_mode_e last_chassis_mode;
-static float rotate_speed = 70000;
+
 /* 机器人底盘控制核心任务 */
-void ChassisTask() {
+void ChassisTask()
+{
     // 后续增加没收到消息的处理(双板的情况)
     // 获取新的控制信息
 #ifdef ONE_BOARD
@@ -276,13 +264,15 @@ 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) { // 如果出现重要模块离线或遥控器设置为急停,让电机停止
+    if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
+    { // 如果出现重要模块离线或遥控器设置为急停,让电机停止
         DJIMotorStop(motor_lf);
         DJIMotorStop(motor_rf);
         DJIMotorStop(motor_lb);
         DJIMotorStop(motor_rb);
-    } else { // 正常工作
+    }
+    else
+    { // 正常工作
         DJIMotorEnable(motor_lf);
         DJIMotorEnable(motor_rf);
         DJIMotorEnable(motor_lb);
@@ -290,79 +280,80 @@ void ChassisTask() {
     }
 
     // 根据控制模式设定旋转速度
-    switch (chassis_cmd_recv.chassis_mode) {
-        case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态
-            chassis_cmd_recv.wz = 0;
-            break;
-        case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
-            chassis_cmd_recv.wz = 40.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
-            LIMIT_MIN_MAX(chassis_cmd_recv.wz,-35000,35000);
-            break;
-        case CHASSIS_SIDEWAYS: // 侧向,不单独设置pid,以误差角度平方为速度输出
-            chassis_cmd_recv.wz = 20.0f * (chassis_cmd_recv.offset_angle - 45)* abs(chassis_cmd_recv.offset_angle - 45);
-            LIMIT_MIN_MAX(chassis_cmd_recv.wz,-35000,35000);
-            break;
-        case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
-            if(last_chassis_mode != CHASSIS_ROTATE){
-                rotate_speed = -rotate_speed;
-            }
-            chassis_cmd_recv.wz = rotate_speed;
-            break;
-        default:
-            break;
+    switch (chassis_cmd_recv.chassis_mode)
+    {
+    case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态
+        chassis_cmd_recv.wz = 0;
+        break;
+    case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
+        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 = 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;
     }
 
-    last_chassis_mode = chassis_cmd_recv.chassis_mode;
     // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
     // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
     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);
 
-    //一阶低通滤波计算
-    first_order_filter_cali(&vx_filter, chassis_cmd_recv.vx);
-    first_order_filter_cali(&vy_filter, chassis_cmd_recv.vy);
-
-    chassis_cmd_recv.vx = vx_filter.out;
-    chassis_cmd_recv.vy = vy_filter.out;
-
     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;
 
-//    chassis_vx = (1.0f - 0.30f) * chassis_vx + 0.30f * (chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta);
-//    chassis_vy = (1.0f - 0.30f) * chassis_vy + 0.30f * (chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta);
+
+
 
 
     // 根据控制模式进行正运动学解算,计算底盘输出
-    //MecanumCalculate();
-    OmniCalculate();
-    ////对缓冲功率进行闭环
+    MecanumCalculate();
+    //读取底盘功率，方便功率控制
+//    chassis_power = PowerMeterGet(power_meter);
+
+    //更新超电设定值
     SuperCapSetUpdate();
     // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
     LimitChassisOutput();
-
-//    float vofa_send_data[2];
-//    vofa_send_data[0] = motor_lb->motor_controller.speed_PID.Ref;
-//    vofa_send_data[1] = motor_lb->motor_controller.speed_PID.Measure;
-//    vofa_justfloat_output(vofa_send_data, 8, &huart1);
-
     // 根据电机的反馈速度和IMU(如果有)计算真实速度
     EstimateSpeed();
 
-    //todo: 裁判系统信息移植到消息中心发送
-    // 获取裁判系统数据   建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
-    // 发送敌方方颜色id
+
+
+    // // 获取裁判系统数据   建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
+    // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
     //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;
+
+    // // 当前只做了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);
+    PubPushMessage(chassis_pub, (void *)&chassis_feedback_data);
 #endif
 #ifdef CHASSIS_BOARD
     CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data);
 #endif // CHASSIS_BOARD
-}
+}

application/cmd/robot_cmd.c

@@ -3,55 +3,51 @@
 #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 "super_cap.h"
-#include "user_lib.h"
+#include "vision_transfer.h"
 // bsp
 #include "bsp_dwt.h"
 #include "bsp_log.h"
-#include "referee_task.h"
-#include "vision_transfer.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应用包含的模块实例指针和交互信息存储*/
 #ifdef GIMBAL_BOARD // 对双板的兼容,条件编译
 #include "can_comm.h"
-#include "user_lib.h"
-
 static CANCommInstance *cmd_can_comm; // 双板通信
 #endif
 #ifdef ONE_BOARD
 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; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等
-extern SuperCapInstance *cap;             // 超级电容
-static RC_ctrl_t *rc_data;                // 遥控器数据,初始化时返回
-static VT_ctrl_t *vt_data;               // 图传链路下发的键鼠遥控数据 与遥控器数据格式保持一致
 
+static RC_ctrl_t *rc_data;              // 遥控器数据,初始化时返回
 //static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回
 //static Vision_Send_s vision_send_data;  // 视觉发送数据
-static RecievePacket_t *vision_recv_data; // 视觉接收数据指针,初始化时返回
-static SendPacket_t vision_send_data;     // 视觉发送数据
+static VT_ctrl_t *vt_data;               // 图传链路下发的键鼠遥控数据 与遥控器数据格式保持一致
+RecievePacket_t *vision_recv_data; // 视觉接收数据指针,初始化时返回
+SendPacket_t vision_send_data;  // 视觉发送数据
 
 //自瞄相关信息
-static Trajectory_Type_t trajectory_cal;
-static Aim_Select_Type_t aim_select;
-static uint32_t no_find_cnt;      // 未发现目标计数
+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 uint8_t choose_amor_id = 0;
-static float yaw_err_for_test;
 
 static Publisher_t *gimbal_cmd_pub;            // 云台控制消息发布者
 static Subscriber_t *gimbal_feed_sub;          // 云台反馈信息订阅者
@@ -64,16 +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;
 
-static referee_info_t *referee_data; // 用于获取裁判系统的数据
-static uint8_t loader_flag = 0;      //拨弹模式选择标志位
+float PitchMotorAngle;
+float DeltaPitchAngle;
 
-void RobotCMDInit() {
+void RobotCMDInit()
+{
     rc_data = RemoteControlInit(&huart3);   // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
-    vision_recv_data = VisionInit(); // 视觉通信串口
-    vt_data = VTRefereeInit(&huart1);   // 图传通信串口
-    referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI
-
+    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));
@@ -95,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; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入
 }
@@ -105,7 +106,8 @@ void RobotCMDInit() {
  *        单圈绝对角度的范围是0~360,说明文档中有图示
  *
  */
-static void CalcOffsetAngle() {
+static void CalcOffsetAngle()
+{
     // 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数
     static float angle;
     angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度
@@ -124,41 +126,49 @@ 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 ||
-        referee_data->GameRobotState.power_management_chassis_output == 0) {
+static void death_check()
+{
+    if(referee_data->GameRobotState.current_HP <= 0)
+    {
         rc_data[TEMP].key_count[KEY_PRESS][Key_E] = 0;
-        gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle;
     }
 }
 
-//功能：等级提升弹频提高
-static void shoot_rate_improve() {
-    if (referee_data->GameRobotState.robot_level < 5)
-        shoot_cmd_send.shoot_rate = 12;
-    if (referee_data->GameRobotState.robot_level >= 5)
-        shoot_cmd_send.shoot_rate = 25;
-}
-
-static void update_ui_data() {
+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.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.lid_mode = shoot_cmd_send.lid_mode;
-    ui_data.heat_mode = shoot_cmd_send.heat_mode;
-    ui_data.aim_fire = aim_select.suggest_fire;
-    //ui_data.loader_mode = shoot_cmd_send.loader_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.Cap_Data.cap_vol = chassis_fetch_data.cap_vol;
+    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);
 
-static void auto_aim_mode() {
-    trajectory_cal.v0 = 26; //弹速30
+//    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;
@@ -176,51 +186,75 @@ static void auto_aim_mode() {
         no_find_cnt = 0;
         auto_aim_flag = 1;
 
-        choose_amor_id = auto_aim(&aim_select, &trajectory_cal, vision_recv_data);
-
-        VisionSetAim(aim_select.aim_point[0], aim_select.aim_point[1], aim_select.aim_point[2]);
+        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;
-        float yaw_err = diff_yaw;
-        yaw_err_for_test = yaw_err;
-
-        if (diff_yaw > 180)
+        if(diff_yaw>180)
             diff_yaw -= 360;
-        else if (diff_yaw < -180)
+        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;
+        gimbal_cmd_send.pitch = -trajectory_cal.cmd_pitch * 180 / PI + 0.2 * trajectory_cal.dis;
 
-        if (yaw_err <= 2) //3度
-        {
-            aim_select.suggest_fire = 1;
-        } else
-            aim_select.suggest_fire = 0;
+        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 控制输入为遥控器(调试时)的模式和控制量设置
  *
  */
-static void RemoteControlSet() {
+static void RemoteControlSet()
+{
+    aim_select.suggest_fire = 0;
     // 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据?
     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_FOLLOW_GIMBAL_YAW;
         gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
-        shoot_cmd_send.lid_mode = LID_CLOSE;
-    } else if (switch_is_up(rc_data[TEMP].rc.switch_right))// 右侧开关状态[上]
-    {
-        shoot_cmd_send.lid_mode = LID_OPEN;
+        shoot_cmd_send.tele_mode = TELE_CLOSE;
     }
 
     // 云台参数,确定云台控制数据
@@ -228,19 +262,11 @@ static void RemoteControlSet() {
         (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的增量进行控制
-        // ...
-        aim_select.suggest_fire = 0;
-        gimbal_cmd_send.yaw -= 0.0025f * (float) rc_data[TEMP].rc.rocker_l_;
-        gimbal_cmd_send.pitch -= 0.001f * (float) rc_data[TEMP].rc.rocker_l1;
-
-        float delta_yaw_total = gimbal_cmd_send.yaw - gimbal_fetch_data.gimbal_imu_data.YawTotalAngle;
-        float delta_yaw = theta_format(delta_yaw_total);
-        gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle + delta_yaw;
-
-        if (gimbal_cmd_send.pitch >= PITCH_MAX_ANGLE) gimbal_cmd_send.pitch = PITCH_MAX_ANGLE;
-        if (gimbal_cmd_send.pitch <= PITCH_MIN_ANGLE) gimbal_cmd_send.pitch = PITCH_MIN_ANGLE;
+        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();
 
     }
     // 左侧开关状态为[下],视觉模式
@@ -249,253 +275,229 @@ static void RemoteControlSet() {
     }
     // 云台软件限位
 
-    // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整，遥控器输入灵敏度
-    chassis_cmd_send.vx = 0.001f * (float) rc_data[TEMP].rc.rocker_r_; // _水平方向
-    chassis_cmd_send.vy = 0.001f * (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模块,开启
-    else; // 弹舱舵机控制,待添加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模块,关闭
 
     // 摩擦轮控制,拨轮向上打为负,向下为正
     if (rc_data[TEMP].rc.dial < -100) // 向上超过100,打开摩擦轮
         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.loader_mode = LOAD_BURSTFIRE;
+    if (rc_data[TEMP].rc.dial < -500)
+        shoot_cmd_send.load_mode = LOAD_1_BULLET;
     else
-        shoot_cmd_send.loader_mode = LOAD_STOP;
-
-    // 视觉控制发射
-//    if (aim_select.suggest_fire == 1) {
-//        //shoot_cmd_send.friction_mode = FRICTION_ON;
-//        //shoot_cmd_send.shoot_mode = SHOOT_ON;
-//        shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
-//    } else {
-//        //shoot_cmd_send.friction_mode = FRICTION_OFF;
-//        shoot_cmd_send.load_mode = LOAD_STOP;
-//    }
-
+        shoot_cmd_send.load_mode = LOAD_STOP;
     // 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频,
-    shoot_cmd_send.shoot_rate = 18;
-
-    if (shoot_fetch_data.stalled_flag == 1)
-        shoot_cmd_send.loader_mode = LOAD_REVERSE;
-}
-
-static void hand_aim_mode() {
-//    gimbal_cmd_send.yaw += (float) rc_data[TEMP].mouse.x / 660 * 6; // 系数待测
-//    gimbal_cmd_send.pitch -= (float) rc_data[TEMP].mouse.y / 660 * 6;
+    shoot_cmd_send.shoot_rate = 500;
 }
 
 /**
  * @brief 输入为键鼠时模式和控制量设置
  *
  */
-static void MouseKeySet() {
-    chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 1 - rc_data[TEMP].key[KEY_PRESS].s * 1; // 系数待测
-    chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 1 - rc_data[TEMP].key[KEY_PRESS].d * 1;
+static void MouseKeySet()
+{
+    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:
+            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;
+    }
 
-    gimbal_cmd_send.yaw -= (float) rc_data[TEMP].mouse.x / 660 * 4; // 系数待测
-    gimbal_cmd_send.pitch += (float) rc_data[TEMP].mouse.y / 660 * 6;
-
-    float delta_yaw_total = gimbal_cmd_send.yaw - gimbal_fetch_data.gimbal_imu_data.YawTotalAngle;
-    float delta_yaw = theta_format(delta_yaw_total);
-    gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle + delta_yaw;
-
-    if (gimbal_cmd_send.pitch >= PITCH_MAX_ANGLE) gimbal_cmd_send.pitch = PITCH_MAX_ANGLE;
-    if (gimbal_cmd_send.pitch <= PITCH_MIN_ANGLE) gimbal_cmd_send.pitch = PITCH_MIN_ANGLE;
 
     aim_select.suggest_fire = 0;
     if (rc_data[TEMP].mouse.press_l && (!rc_data[TEMP].mouse.press_r)) // 左键发射模式
     {
         if (shoot_cmd_send.friction_mode == FRICTION_ON) {
-            shoot_cmd_send.shoot_mode = SHOOT_ON;
-            if (loader_flag == 0) {
-                shoot_cmd_send.loader_mode = LOAD_BURSTFIRE;
-            } else if (loader_flag == 1) {
-                shoot_cmd_send.loader_mode = LOAD_3_BULLET;
-            } else
-                shoot_cmd_send.loader_mode = LOAD_1_BULLET;
+            shoot_cmd_send.load_mode = LOAD_1_BULLET;
         }
-    } else if ((!rc_data[TEMP].mouse.press_l) && (!rc_data[TEMP].mouse.press_r)) {
-        shoot_cmd_send.loader_mode = LOAD_STOP;
+
+    } else if ((!rc_data[TEMP].mouse.press_l) && (!rc_data[TEMP].mouse.press_r))
+    {
+        shoot_cmd_send.load_mode = LOAD_STOP;
+
     }
+
     if (rc_data[TEMP].mouse.press_r) // 右键自瞄模式
     {
         if ((vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0
              && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 &&
-             vision_recv_data->vz == 0)) {
-            shoot_cmd_send.loader_mode = LOAD_STOP;
-        } else {
+             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.shoot_mode = SHOOT_ON;
-                shoot_cmd_send.loader_mode = LOAD_BURSTFIRE;
+                shoot_cmd_send.load_mode = LOAD_1_BULLET;
             } else {
-                shoot_cmd_send.loader_mode = LOAD_STOP;
+                shoot_cmd_send.load_mode = LOAD_STOP;
             }
         }
     }
 
+    switch (rc_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 (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:
+        break;
+    }
+    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_B] % 2) // B键加100转速
     {
         case 1:
-            MyUIInit();
-            rc_data[TEMP].key_count[KEY_PRESS][Key_R]++;
+            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_V] % 2) // V键开关红点激光
+    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_C] % 2) // C键减100转速
     {
-        case 0:
-            HAL_GPIO_WritePin(GPIOC, GPIO_PIN_8, GPIO_PIN_SET);
+        case 1:
+            shoot_cmd_send.motor_speed -=100;
+            rc_data[TEMP].key_count[KEY_PRESS][Key_C]++;
             break;
         default:
-            HAL_GPIO_WritePin(GPIOC, GPIO_PIN_8, GPIO_PIN_RESET);
             break;
     }
     switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮
     {
-        case 0:
-            shoot_cmd_send.shoot_mode = SHOOT_OFF;
-            shoot_cmd_send.friction_mode = FRICTION_OFF;
-            break;
-        default:
-            shoot_cmd_send.shoot_mode = SHOOT_ON;
-            shoot_cmd_send.friction_mode = FRICTION_ON;
-            break;
+    case 0:
+        shoot_cmd_send.friction_mode = FRICTION_OFF;
+        break;
+    default:
+        shoot_cmd_send.friction_mode = FRICTION_ON;
+        break;
     }
-    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_B] % 2) // B键开关弹舱盖
+    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q键开关倍镜
     {
         case 0:
-            shoot_cmd_send.lid_mode = LID_CLOSE;
-            break;
-        default:
-            shoot_cmd_send.lid_mode = LID_OPEN;
-            break;
-    }
-//    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺
-//    {
-//        case 0:
-//            chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
-//            gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
-//            break;
-//        default:
-//            chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
-//            gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
-//            break;
-//    }
-    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q键开关热控
-    {
-        case 0:
-            shoot_cmd_send.heat_mode = HEAT_OPEN;
-            break;
-        default:
-            shoot_cmd_send.heat_mode = HEAT_CLOSE;
-            break;
-    }
-    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_C] % 2) // C键侧向
-    {
-        case 0:
-            switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺
-            {
-                case 0:
-                    chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
-                    gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
-                    break;
-                default:
-                    chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
-                    gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
-                    break;
-            }
+            shoot_cmd_send.tele_mode = TELE_CLOSE;
             break;
         case 1:
-            chassis_cmd_send.chassis_mode = CHASSIS_SIDEWAYS;
+            shoot_cmd_send.tele_mode = TELE_OPEN;
             break;
     }
-//    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_G] % 3) // G键切换发射模式
-//    {
-//        case 0:
-//            loader_flag = 0;
-//            break;
-//        case 1:
-//            loader_flag = 1;
-//            break;
-//        default:
-//            loader_flag = 3;
-//            break;
-//    }
-    switch (rc_data[TEMP].key[KEY_PRESS].shift) //按shift允许消耗超级电容能量
+    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺
     {
         case 0:
-            chassis_cmd_send.buffer_supercap = 5;
+            chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
             break;
-        case 1:
-            chassis_cmd_send.buffer_supercap = 200;
-            chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 15000 - rc_data[TEMP].key[KEY_PRESS].d * 15000;
+        default:
+            chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
             break;
     }
-
-    shoot_rate_improve();
-
-    if (shoot_fetch_data.stalled_flag == 1)
-        shoot_cmd_send.loader_mode = LOAD_REVERSE;
-
-    death_check();
+    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 输入为（图传链路）键鼠时模式和控制量设置
+	 * @brief 输入为（图传链路）键鼠时模式和控制量设置
  *
  */
-static void VTMouseKeySet() {
-    chassis_cmd_send.vy =
-            vt_data[TEMP].key[KEY_PRESS].w * 1 - vt_data[TEMP].key[KEY_PRESS].s * 1; // 系数待测 小 3000 3000
-    chassis_cmd_send.vx = vt_data[TEMP].key[KEY_PRESS].a * 1 - vt_data[TEMP].key[KEY_PRESS].d * 1;
+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 * 4; // 系数待测
-    gimbal_cmd_send.pitch += (float) vt_data[TEMP].mouse.y / 660 * 6;
+    gimbal_cmd_send.yaw -= (float)vt_data[TEMP].mouse.x / 660 * 10; // 系数待测
+    gimbal_cmd_send.pitch += (float)vt_data[TEMP].mouse.y / 660 * 10;
 
-    if (gimbal_cmd_send.pitch >= PITCH_MAX_ANGLE) gimbal_cmd_send.pitch = PITCH_MAX_ANGLE;
-    if (gimbal_cmd_send.pitch <= PITCH_MIN_ANGLE) gimbal_cmd_send.pitch = PITCH_MIN_ANGLE;
 
     aim_select.suggest_fire = 0;
     if (vt_data[TEMP].mouse.press_l && (!vt_data[TEMP].mouse.press_r)) // 左键发射模式
     {
         if (shoot_cmd_send.friction_mode == FRICTION_ON) {
-            shoot_cmd_send.shoot_mode = SHOOT_ON;
-            shoot_cmd_send.loader_mode = LOAD_BURSTFIRE;
+            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.loader_mode = LOAD_STOP;
+
+    } 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.loader_mode = LOAD_STOP;
-        } else {
+             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.shoot_mode = SHOOT_ON;
-                shoot_cmd_send.loader_mode = LOAD_BURSTFIRE;
+                shoot_cmd_send.load_mode = LOAD_1_BULLET;
             } else {
-                shoot_cmd_send.loader_mode = LOAD_STOP;
+                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:
@@ -505,103 +507,66 @@ static void VTMouseKeySet() {
         default:
             break;
     }
-    switch (vt_data[TEMP].key_count[KEY_PRESS][Key_V] % 2) // V键开关红点激光
-    {
-        case 0:
-            HAL_GPIO_WritePin(GPIOC, GPIO_PIN_8, GPIO_PIN_SET);
-            break;
-        default:
-            HAL_GPIO_WritePin(GPIOC, GPIO_PIN_8, GPIO_PIN_RESET);
-            break;
-    }
     switch (vt_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮
     {
         case 0:
-            shoot_cmd_send.shoot_mode = SHOOT_OFF;
             shoot_cmd_send.friction_mode = FRICTION_OFF;
             break;
         default:
-            shoot_cmd_send.shoot_mode = SHOOT_ON;
             shoot_cmd_send.friction_mode = FRICTION_ON;
             break;
     }
-    switch (vt_data[TEMP].key_count[KEY_PRESS][Key_B] % 2) // B键开关弹舱盖
+    switch (vt_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q键开关倍镜
     {
         case 0:
-            shoot_cmd_send.lid_mode = LID_CLOSE;
+            shoot_cmd_send.tele_mode = TELE_CLOSE;
             break;
-        default:
-            shoot_cmd_send.lid_mode = LID_OPEN;
+        case 1:
+            shoot_cmd_send.tele_mode = TELE_OPEN;
             break;
     }
-//    switch (vt_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺
+    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;
-//            gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
-//            break;
-//        default:
-//            chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
-//            gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
-//            break;
-//    }
-    switch (vt_data[TEMP].key_count[KEY_PRESS][Key_C] % 2) // C键侧向
-    {
-        case 0:
-            switch (vt_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺
-            {
-                case 0:
-                    chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
-                    gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
-                    break;
-                default:
-                    chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
-                    gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
-                    break;
-            }
-            break;
-        case 1:
-            chassis_cmd_send.chassis_mode = CHASSIS_SIDEWAYS;
-            break;
-    }
-    switch (vt_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q键开关热控
-    {
-        case 0:
-            shoot_cmd_send.heat_mode = HEAT_OPEN;
-            break;
-        default:
-            shoot_cmd_send.heat_mode = HEAT_CLOSE;
-            break;
-    }
-//    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_G] % 3) // G键切换发射模式
-//    {
-//        case 0:
-//            loader_flag = 0;
 //            break;
 //        case 1:
-//            loader_flag = 1;
-//            break;
-//        default:
-//            loader_flag = 3;
+//            chassis_cmd_send.chassis_mode = CHASSIS_VERTICAL_YAW;
 //            break;
 //    }
-    switch (vt_data[TEMP].key[KEY_PRESS].shift) //按shift允许消耗超级电容能量
+    switch (vt_data[TEMP].key[KEY_PRESS].shift) // 待添加 按shift允许超功率 消耗缓冲能量
     {
-        case 0:
-            chassis_cmd_send.buffer_supercap = 5;
-            break;
         case 1:
-            chassis_cmd_send.buffer_supercap = 200;
-            chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 1 - rc_data[TEMP].key[KEY_PRESS].d * 1;
+
+            break;
+
+        default:
+
             break;
     }
 
-    shoot_rate_improve();
+    pitch_limit();
 
-    if (shoot_fetch_data.stalled_flag == 1)
-        shoot_cmd_send.loader_mode = LOAD_REVERSE;
-
-    death_check();
 }
 
 
@@ -612,7 +577,8 @@ static void VTMouseKeySet() {
  * @todo   后续修改为遥控器离线则电机停止(关闭遥控器急停),通过给遥控器模块添加daemon实现
  *
  */
-static void EmergencyHandler() {
+static void EmergencyHandler()
+{
     // 拨轮的向下拨超过一半进入急停模式.注意向打时下拨轮是正
     if (rc_data[TEMP].rc.dial > 300 || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断
     {
@@ -621,79 +587,64 @@ static void EmergencyHandler() {
         chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
         shoot_cmd_send.shoot_mode = SHOOT_OFF;
         shoot_cmd_send.friction_mode = FRICTION_OFF;
-        shoot_cmd_send.loader_mode = LOAD_STOP;
+        shoot_cmd_send.load_mode = LOAD_STOP;
         LOGERROR("[CMD] emergency stop!");
     }
     // 遥控器右侧开关为[上],恢复正常运行
-    if (switch_is_up(rc_data[TEMP].rc.switch_right)) {
+    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;
-        gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle;
+        shoot_cmd_send.friction_mode = FRICTION_OFF;
+
+
         LOGINFO("[CMD] reinstate, robot ready");
     }
 }
 
 /* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */
-void RobotCMDTask() {
+void RobotCMDTask()
+{
     // 从其他应用获取回传数据
 #ifdef ONE_BOARD
-    SubGetMessage(chassis_feed_sub, (void *) &chassis_fetch_data);
+    SubGetMessage(chassis_feed_sub, (void *)&chassis_fetch_data);
 #endif // ONE_BOARD
 #ifdef GIMBAL_BOARD
     chassis_fetch_data = *(Chassis_Upload_Data_s *)CANCommGet(cmd_can_comm);
 #endif // GIMBAL_BOARD
     SubGetMessage(shoot_feed_sub, &shoot_fetch_data);
     SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);
+    //更新UI
     update_ui_data();
     // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成
     CalcOffsetAngle();
-    EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
-    if (robot_state != ROBOT_STOP) {
-        // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠
-        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) &&
-                 switch_is_mid(rc_data[TEMP].rc.switch_right)) // 遥控器左侧开关状态为[上],键盘控制,遥控器右侧开关状态为[中],遥控器通道
-            MouseKeySet();
-        else if (switch_is_up(rc_data[TEMP].rc.switch_left) &&
-                 switch_is_up(rc_data[TEMP].rc.switch_right)) // 遥控器左侧开关状态为[上],键盘控制，遥控器右侧开关状态为[上],图传链路通道
-            VTMouseKeySet();
+    // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠
+    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(!referee_data->referee_id.Robot_Color);
-
-    //根据裁判系统反馈确定底盘功率上限
+    //根据裁判系统反馈确定底盘功率上限和缓冲功率
     chassis_cmd_send.chassis_power_limit = referee_data->GameRobotState.chassis_power_limit;
-    //根据裁判系统反馈确定缓冲功率
-    chassis_cmd_send.buffer_energy = referee_data->PowerHeatData.buffer_energy;
-    //根据裁判系统反馈确定枪管热量控制
-    shoot_cmd_send.heat = referee_data->PowerHeatData.shooter_17mm_1_barrel_heat;
-    shoot_cmd_send.heat_limit = referee_data->GameRobotState.shooter_barrel_heat_limit;
-    shoot_cmd_send.heat_cool = referee_data->GameRobotState.shooter_barrel_cooling_value;
-
-    if (referee_data->GameRobotState.power_management_chassis_output == 0) {
-        chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
-    }
-    if (referee_data->GameRobotState.power_management_gimbal_output == 0) {
-        gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
-    }
-    if (referee_data->GameRobotState.power_management_shooter_output == 0) {
-        shoot_cmd_send.shoot_mode = SHOOT_OFF;
-    }
-
+    chassis_cmd_send.chassis_power_buffer = referee_data->PowerHeatData.buffer_energy;
     death_check();
-
     // 推送消息,双板通信,视觉通信等
     // 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置
 #ifdef ONE_BOARD
-    PubPushMessage(chassis_cmd_pub, (void *) &chassis_cmd_send);
+    PubPushMessage(chassis_cmd_pub, (void *)&chassis_cmd_send);
 #endif // ONE_BOARD
 #ifdef GIMBAL_BOARD
     CANCommSend(cmd_can_comm, (void *)&chassis_cmd_send);
 #endif // GIMBAL_BOARD
-    PubPushMessage(shoot_cmd_pub, (void *) &shoot_cmd_send);
-    PubPushMessage(gimbal_cmd_pub, (void *) &gimbal_cmd_send);
-
+    PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
+    PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
 }

application/cmd/robot_cmd.h

@@ -2,6 +2,9 @@
 #define ROBOT_CMD_H
 
 
+
+
+
 /**
  * @brief 机器人核心控制任务初始化,会被RobotInit()调用
  * 

application/gimbal/gimbal.c

@@ -4,197 +4,163 @@
 #include "ins_task.h"
 #include "message_center.h"
 #include "general_def.h"
+
 #include "bmi088.h"
-#include "vofa.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的控制信息
 
-static float gravity_current = 0;
-static void LimitPitchAngleAuto() {
-    /** 注意电机角度与陀螺仪角度方向是否相同
-     * 目前 add > 0, 向下转动
-     * 电机角度值减小
-     * 陀螺仪角度值增大
-     **/
-    float add;
-    float angle_set;
 
-    add = gimbal_cmd_recv.pitch - gimba_IMU_data->Pitch;
-
-    if(pitch_motor->measure.angle_single_round - add > PITCH_MAX_RELATIVE_ANGLE){
-        if(add < 0.0f ){
-            add = PITCH_MAX_ANGLE - pitch_motor->measure.angle_single_round;
-        }
-    }else if(pitch_motor->measure.angle_single_round - add < PITCH_MIN_RELATIVE_ANGLE){
-        if(add > 0.0f){
-            add = PITCH_MIN_RELATIVE_ANGLE - pitch_motor->measure.angle_single_round;
-        }
-    }
-    angle_set = gimba_IMU_data->Pitch;
-    DJIMotorSetRef(pitch_motor, angle_set+add);
-}
-
-void GimbalInit() {
+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_init_config = {
+            .can_handle = &hcan2,
+            .tx_id = 0x01,
+            .rx_id = 0x43,
+        },
+        .controller_param_init_config = {
+            .angle_PID = {
+                .Kp = 2.0f,//0.5, // 8
+                .Ki = 0,
+                .Kd = 0.02f,
+                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
+                .IntegralLimit = 100,
+                .MaxOut = 500,
             },
-            .controller_param_init_config = {
-                    .angle_PID = {
-                            .Kp = 0.8f,//1.2
-                            .Ki = 0.0f,//0
-                            .Kd = 0.04f,//0.05
-                            .DeadBand = 0.0f,
-                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
-                            .IntegralLimit = 100,
-                            .MaxOut = 500,
-                    },
-                    .speed_PID = {
-                            .Kp = 5000,  //4000
-                            .Ki = 100.0f, //100
-                            .Kd = 0.03f, //0
-                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
-                            .IntegralLimit = 10000,
-                            .MaxOut = 15000,
-                    },
-                    .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
-                    // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
-                    .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2],
+            .speed_PID = {
+                .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,
             },
-            .controller_setting_init_config = {
-                    .angle_feedback_source = OTHER_FEED,
-                    .speed_feedback_source = OTHER_FEED,
-                    .outer_loop_type = ANGLE_LOOP,
-                    .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
-                    .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
-            },
-            .motor_type = GM6020,
-            .motor_control_type = CURRENT_CONTROL
+            .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
+            // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
+            .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2],
+        },
+        .controller_setting_init_config = {
+            .angle_feedback_source = OTHER_FEED,
+            .speed_feedback_source = OTHER_FEED,
+            .outer_loop_type = ANGLE_LOOP,
+            .close_loop_type =  ANGLE_LOOP |SPEED_LOOP,
+            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
+        },
+        .motor_type = DM4310
     };
     // PITCH
     Motor_Init_Config_s pitch_config = {
-            .can_init_config = {
-                    .can_handle = &hcan2,
-                    .tx_id = 1,
+        .can_init_config = {
+            .can_handle = &hcan2,
+            .tx_id = 2,
+        },
+        .controller_param_init_config = {
+            .angle_PID = {
+                .Kp = 0.35f,
+                .Ki = 0,
+                .Kd = 0.005f,
+                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
+                .IntegralLimit = 100,//100
+                .MaxOut = 500,//500
             },
-            .controller_param_init_config = {
-                    .angle_PID = {
-                            .Kp = 1.0f,
-                            .Ki = 0.0f,
-                            .Kd = 0.02f,
-                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
-                            .IntegralLimit = 100,
-                            .MaxOut = 500,
-                    },
-                    .speed_PID = {
-                            .Kp = 6000.0f,
-                            .Ki = 900.0f,
-                            .Kd = 0.0f,   // 0
-                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
-                            .IntegralLimit = 10000,
-                            .MaxOut = 15000,
-                    },
-                    .other_angle_feedback_ptr = &gimba_IMU_data->Roll,
-                    // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
-                    .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[1],
-                    .current_feedforward_ptr = &gravity_current,
+            .speed_PID = {
+                .Kp = 6000,
+                .Ki = 1000, // 350
+                .Kd = 0,//15000,   // 0
+                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
+                .IntegralLimit = 2500,
+                .MaxOut = 20000,
             },
-            .controller_setting_init_config = {
-                    .angle_feedback_source = OTHER_FEED,
-                    .speed_feedback_source = OTHER_FEED,
-                    .outer_loop_type = ANGLE_LOOP,
-                    .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
-                    .motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
-                    .feedforward_flag = CURRENT_FEEDFORWARD,
-                    .feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE
-            },
-            .motor_type = GM6020,
-            .motor_control_type = CURRENT_CONTROL
+            .other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
+            // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
+            .other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[0]),
+        },
+        .controller_setting_init_config = {
+            .angle_feedback_source = OTHER_FEED,
+            .speed_feedback_source = OTHER_FEED,
+            .outer_loop_type = ANGLE_LOOP,
+            .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
+            .motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
+            .feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE,
+        },
+        .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() {
-    //红点激光
-    //HAL_GPIO_WritePin(GPIOC,GPIO_PIN_8,GPIO_PIN_SET);
+void GimbalTask()
+{
     // 获取云台控制数据
     // 后续增加未收到数据的处理
     SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
 
     // @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘
     // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref
-    switch (gimbal_cmd_recv.gimbal_mode) {
-        // 停止
-        case GIMBAL_ZERO_FORCE:
-            DJIMotorStop(yaw_motor);
-            DJIMotorStop(pitch_motor);
-            break;
-            // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用
-        case GIMBAL_GYRO_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);
-            //LimitPitchAngleAuto();
-            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);
-            break;
-        default:
-            break;
+    switch (gimbal_cmd_recv.gimbal_mode)
+    {
+    // 停止
+    case GIMBAL_ZERO_FORCE:
+        DMMotorStop(yaw_motor);
+        DJIMotorStop(pitch_motor);
+        break;
+    // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用
+    case GIMBAL_GYRO_MODE: // 后续只保留此模式
+        YawMotorEnable();
+        DMMotorEnable(yaw_motor);
+        DJIMotorEnable(pitch_motor);
+        DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED);
+        DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
+        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);
+        break;
+    default:
+        break;
     }
 
     // 在合适的地方添加pitch重力补偿前馈力矩
     // 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩
     // ...
-    float theta = - gimba_IMU_data->Roll/180*PI;
-    gravity_current = (5000)*arm_cos_f32(theta);
-
-//    float vofa_send_data[4];
-//    vofa_send_data[0] = pitch_motor->motor_controller.speed_PID.Ref;
-//    vofa_send_data[1] = pitch_motor->motor_controller.speed_PID.Measure;
-//    vofa_send_data[2] = pitch_motor->motor_controller.angle_PID.Ref;
-//    vofa_send_data[3] = pitch_motor->motor_controller.angle_PID.Measure;
-//    vofa_justfloat_output(vofa_send_data, 16, &huart1);
-//    float vofa_send_data[4];
-//    vofa_send_data[0] = yaw_motor->motor_controller.speed_PID.Ref;
-//    vofa_send_data[1] = yaw_motor->motor_controller.speed_PID.Measure;
-//    vofa_send_data[2] = yaw_motor->motor_controller.angle_PID.Ref;
-//    vofa_send_data[3] = yaw_motor->motor_controller.angle_PID.Measure;
-//    vofa_justfloat_output(vofa_send_data, 16, &huart1);
-
 
     // 设置反馈数据,主要是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);
+
+    PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
 }

application/robot.c

@@ -21,17 +21,18 @@
 
 
 void RobotInit()
-{
+{  
     // 关闭中断,防止在初始化过程中发生中断
     // 请不要在初始化过程中使用中断和延时函数！
     // 若必须,则只允许使用DWT_Delay()
     __disable_irq();
-
+    
     BSPInit();
 
+
+
 #if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
     RobotCMDInit();
-
     GimbalInit();
     ShootInit();
 #endif
@@ -40,6 +41,7 @@ void RobotInit()
     ChassisInit();
 #endif
 
+
     OSTaskInit(); // 创建基础任务
 
     // 初始化完成,开启中断
@@ -50,7 +52,6 @@ void RobotTask()
 {
 #if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
     RobotCMDTask();
-    //暂时注释云台和发射任务  调试底盘
     GimbalTask();
     ShootTask();
 #endif

application/robot.h

@@ -1,8 +1,5 @@
 #ifndef ROBOT_H
 #define ROBOT_H
-#ifdef __cplusplus
-extern "C"{
-#endif
 
 /* Robot利用robot_def.h中的宏对不同的机器人进行了大量的兼容,同时兼容了两个开发板(云台板和底盘板)的配置 */
 
@@ -18,8 +15,4 @@ void RobotInit();
  */
 void RobotTask();
 
-#ifdef __cplusplus
-}
-#endif
-
 #endif

application/robot_def.h

@@ -26,25 +26,21 @@
 
 /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
 // 云台参数
-#define YAW_CHASSIS_ALIGN_ECD 2053 // 小 1443 大2053 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
+#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 4422    // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
-#define PITCH_MAX_ANGLE 25           // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
-#define PITCH_MIN_ANGLE -24          // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
-
-#define PITCH_MAX_RELATIVE_ANGLE 123        // 云台相对底盘最大角度
-#define PITCH_MIN_RELATIVE_ANGLE 80          // 云台相对底盘最小角度
-
+#define PITCH_HORIZON_ECD 3412      // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
+#define PITCH_MAX_ANGLE 15           // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
+#define PITCH_MIN_ANGLE -35           // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
 // 发射参数
-#define ONE_BULLET_DELTA_ANGLE 1620    // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
-#define REDUCTION_RATIO_LOADER 36.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f 2006-36.0f
-#define NUM_PER_CIRCLE 8            // 拨盘一圈的装载量
-// 机器人底盘修改的参数,单位为m(米)
-#define WHEEL_BASE 0.1f              // 纵向轴距(前进后退方向)
-#define TRACK_WIDTH 0.1f              // 横向轮距(左右平移方向)
+#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 433.86           // 纵向轴距(前进后退方向)
+#define TRACK_WIDTH 463.92            // 横向轮距(左右平移方向)
 #define CENTER_GIMBAL_OFFSET_X 0    // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0
 #define CENTER_GIMBAL_OFFSET_Y 0    // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0
-#define RADIUS_WHEEL 0.08f            // 轮子半径
+#define RADIUS_WHEEL 75             // 轮子半径
 #define REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换
 
 #define GYRO2GIMBAL_DIR_YAW 1   // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反
@@ -90,7 +86,8 @@ typedef enum
     CHASSIS_ROTATE,            // 小陀螺模式
     CHASSIS_NO_FOLLOW,         // 不跟随，允许全向平移
     CHASSIS_FOLLOW_GIMBAL_YAW, // 跟随模式，底盘叠加角度环控制
-    CHASSIS_SIDEWAYS,          // 侧向
+    CHASSIS_VERTICAL_YAW,
+    CHASSIS_FIXED,
 } chassis_mode_e;
 
 // 云台模式设置
@@ -99,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;
 
 // 发射模式设置
@@ -115,9 +113,9 @@ typedef enum
 
 typedef enum
 {
-    LID_OPEN , // 弹舱盖打开
-    LID_CLOSE,    // 弹舱盖关闭
-} lid_mode_e;
+    TELE_CLOSE = 0, // 倍镜打开
+    TELE_OPEN,    // 倍镜关闭
+} tele_mode_e;
 
 typedef enum
 {
@@ -127,22 +125,15 @@ typedef enum
     LOAD_3_BULLET,  // 三发
     LOAD_BURSTFIRE, // 连发
 } loader_mode_e;
-typedef enum
-{
-    HEAT_OPEN , // 热控打开
-    HEAT_CLOSE, // 热控关闭
-}heat_mode_e;
+
 // 功率限制,从裁判系统获取,是否有必要保留?
 typedef struct
-{
-    uint16_t chassis_power_mx; // 功率限制
+{ // 功率控制
+    uint16_t chassis_power_mx;
+    float cap_vol;
+    uint16_t motor_speed;
 } Chassis_Power_Data_s;
 
-// 电容信息
-typedef struct
-{
-    int16_t cap_vol;
-} Cap_Data_s;
 /* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */
 /**
  * @brief 对于双板情况,遥控器和pc在云台,裁判系统在底盘
@@ -157,12 +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;
-    uint16_t buffer_energy;
-    uint16_t buffer_supercap;
-    uint8_t chassic_flag;
+    uint8_t P_SuperCap;
     // UI部分
     //  ...
 
@@ -174,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;
@@ -182,16 +172,14 @@ typedef struct
 typedef struct
 {
     shoot_mode_e shoot_mode;
-    loader_mode_e loader_mode;
-    lid_mode_e lid_mode;
+    loader_mode_e load_mode;
+    tele_mode_e tele_mode;
     friction_mode_e friction_mode;
-    heat_mode_e heat_mode;
-    Bullet_Speed_e bullet_speed; // 弹速枚举
+    float bullet_speed; // 发射周期
+    uint8_t rest_heat;
     float shoot_rate; // 连续发射的射频,unit per s,发/秒
+    uint16_t motor_speed;
 
-    uint16_t heat;       // 实时热量
-    uint16_t heat_limit;  // 热量上限
-    uint16_t heat_cool;  // 热量每秒冷却值
 } Shoot_Ctrl_Cmd_s;
 
 /* ----------------gimbal/shoot/chassis发布的反馈数据----------------*/
@@ -209,11 +197,10 @@ typedef struct
     // float real_vx;
     // float real_vy;
     // float real_wz;
+    float cap_vol;
+    uint8_t rest_heat;           // 剩余枪口热量
+    Enemy_Color_e enemy_color;   // 0 for blue, 1 for red
 
-    Bullet_Speed_e bullet_speed; // 弹速限制
-    Enemy_Color_e enemy_color;   // 0 for red, 1 for blue
-    uint16_t chassis_power_mx;
-    int16_t cap_vol;
 } Chassis_Upload_Data_s;
 
 
@@ -221,6 +208,7 @@ typedef struct
 {
     attitude_t gimbal_imu_data;
     float yaw_motor_single_round_angle;
+    float pitch_motor_total_angle;
 } Gimbal_Upload_Data_s;
 
 typedef struct
@@ -228,8 +216,9 @@ typedef struct
     // code to go here
     // ...
     uint8_t stalled_flag;  //堵转标志
+    uint8_t Motor_Temperature;
 } Shoot_Upload_Data_s;
 
 #pragma pack() // 开启字节对齐,结束前面的#pragma pack(1)
 
-#endif // !ROBOT_DEF_H
+#endif // !ROBOT_DEF_H

application/robot_task.h

@@ -16,8 +16,7 @@
 #include "buzzer.h"
 
 #include "bsp_log.h"
-
-
+#include "dm4310.h"
 
 osThreadId insTaskHandle;
 osThreadId robotTaskHandle;
@@ -53,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)
@@ -121,9 +120,9 @@ __attribute__((noreturn)) void StartROBOTTASK(void const *argument)
         robot_start = DWT_GetTimeline_ms();
         RobotTask();
         robot_dt = DWT_GetTimeline_ms() - robot_start;
-        if (robot_dt > 1)
+        if (robot_dt > 5)
             LOGERROR("[freeRTOS] ROBOT core Task is being DELAY! dt = [%f]", &robot_dt);
-        osDelay(1);
+        osDelay(5);
     }
 }
 

application/shoot/shoot.c

@@ -4,297 +4,240 @@
 #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 ServoInstance *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,last_hibernate_time = 0, dead_time = 0;
+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,
+        .can_init_config = {
+            .can_handle = &hcan2,
+        },
+        .controller_param_init_config = {
+            .speed_PID = {
+                .Kp = 6, // 20
+                .Ki = 0, // 1
+                .Kd = 0,
+                .Improve = PID_Integral_Limit,
+                .IntegralLimit = 10000,
+                .MaxOut = 28000,
             },
-            .controller_param_init_config = {
-                    .speed_PID = {
-                            .Kp = 1.5f,
-                            .Ki = 0.2f,
-                            .Kd = 0,
-                            .Improve = PID_Integral_Limit,
-                            .IntegralLimit = 10000,
-                            .MaxOut = 15000,
-                    },
-                    .current_PID = {
-                            .Kp = 0,
-                            .Ki = 0,
-                            .Kd = 0,
-                            .Improve = PID_Integral_Limit,
-                            .IntegralLimit = 10000,
-                            .MaxOut = 15000,
-                    },
+            .current_PID = {
+                .Kp = 0, // 0.7
+                .Ki = 0, // 0.1
+                .Kd = 0,
+                .Improve = PID_Integral_Limit,
+                .IntegralLimit = 10000,
+                .MaxOut = 15000,
             },
-            .controller_setting_init_config = {
-                    .angle_feedback_source = MOTOR_FEED,
-                    .speed_feedback_source = MOTOR_FEED,
-                    .outer_loop_type = SPEED_LOOP,
-                    .close_loop_type = SPEED_LOOP,
-            },
-            .motor_type = M3508};
-    friction_config.can_init_config.tx_id = 2,
-            friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
-    friction_l = DJIMotorInit(&friction_config);
+        },
+        .controller_setting_init_config = {
+            .angle_feedback_source = MOTOR_FEED,
+            .speed_feedback_source = MOTOR_FEED,
 
-    friction_config.can_init_config.tx_id = 3; // 右摩擦轮,改txid和方向就行
-    friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
+            .outer_loop_type = SPEED_LOOP,
+            .close_loop_type = SPEED_LOOP ,
+            .motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
+        },
+        .motor_type = M3508};
+//    friction_config.can_init_config.tx_id = 3,
+//    friction_l = DJIMotorInit(&friction_config);
+
+    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 = 1
+        .can_init_config = {
+            .can_handle = &hcan1,
+            .tx_id = 5,
+        },
+        .controller_param_init_config = {
+            .angle_PID = {
+                // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
+                .Kp = 70, // 10
+                .Ki = 0,
+                .Kd = 0.01f,
+                .MaxOut = 15000,
             },
-            .controller_param_init_config = {
-                    .angle_PID = {
-                            // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
-                            .Kp = 500,
-                            .Ki = 100,
-                            .Kd = 0,
-                            .MaxOut = 5000,
-                    },
-                    .speed_PID = {
-                            .Kp = 2, // 3
-                            .Ki = 0, // 1
-                            .Kd = 0,
-                            .Improve = PID_Integral_Limit,
-                            .IntegralLimit = 5000,
-                            .MaxOut = 10000,
-                    },
-                    .current_PID = {
-                            .Kp = 0, // 0.7
-                            .Ki = 0, // 0.1
-                            .Kd = 0,
-                            .Improve = PID_Integral_Limit,
-                            .IntegralLimit = 5000,
-                            .MaxOut = 5000,
-                    },
+            .speed_PID = {
+                .Kp = 3.8f, // 10
+                .Ki = 0, // 1
+                .Kd = 0,
+                .Improve = PID_Integral_Limit,
+                .IntegralLimit = 5000,
+                .MaxOut = 30000,
             },
-            .controller_setting_init_config = {
-                    .angle_feedback_source = MOTOR_FEED,
-                    .speed_feedback_source = MOTOR_FEED,
-                    .outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
-                    .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
-                    .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
-                    //.feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE,
+            .current_PID = {
+                .Kp = 0, // 0.7
+                .Ki = 0, // 0.1
+                .Kd = 0,
+                .Improve = PID_Integral_Limit,
+                .IntegralLimit = 5000,
+                .MaxOut = 15000,
             },
-            .motor_type = M2006 // 英雄使用m3508
+        },
+        .controller_setting_init_config = {
+            .angle_feedback_source = MOTOR_FEED,
+            .speed_feedback_source = MOTOR_FEED,
+            .outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
+            .close_loop_type =   SPEED_LOOP | ANGLE_LOOP,
+            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
+        },
+        .motor_type = M3508 // 英雄使用m3508
     };
     loader = DJIMotorInit(&loader_config);
 
-    shoot_pub = PubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s));
-    shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
-
-    Servo_Init_Config_s lid_config = {
+    // 倍镜舵机
+    Servo_Init_Config_s tele_config = {
             .htim = &htim1,
             .Channel = TIM_CHANNEL_1,
-            .Servo_type = Servo180,
+            .Servo_type = Servo270,
             .Servo_Angle_Type = Free_Angle_mode,
     };
 
-    lid = ServoInit(&lid_config);
+    telescope = ServoInit(&tele_config);
 
-//    SPI_Init_Config_s rgb_config = {
-//           .spi_handle = ;
-//           .GPIOx = GPIOx;
-//           .cs_pin = cs_pin;
-//           .spi_work_mode = spi_work_mode;
-//           .callback = callback;
-//           .id = 1;
-//    }
+    shoot_pub = PubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s));
+    shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
 }
-
-//卡弹检测
-void stalled_detect()
+static void Stop_Check()
 {
-    static float last_detect_time = 0,detect_time = 0;
-    static float last_total_angle = 0;
-    static uint8_t stalled_cnt = 0;
-
-    detect_time = DWT_GetTimeline_ms();
-
-    //last_detect_time + 200 > detect_time
-    if(detect_time - last_detect_time < 200)  // 200ms 检测一次
-        return;
-//    reference_angle = (detect_time - last_detect_time) * loader->motor_controller.speed_PID.Ref * 1e-3f;
-//    real_angle = loader->measure.total_angle - last_total_angle;
-    if(shoot_cmd_recv.loader_mode == LOAD_BURSTFIRE)
+    if(loader->measure.total_angle < stop_location*0.9)
     {
-        //if(real_angle < reference_angle * 0.2f)
-        if(abs(loader->measure.real_current)>=9500)
-        {
-            //stalled_cnt ++;
-            shoot_feedback_data.stalled_flag = 1;
-
-        }
-        last_detect_time = DWT_GetTimeline_ms();
+        DJIMotorOuterLoop(loader, ANGLE_LOOP);
+        DJIMotorSetRef(loader,stop_location);
+        hibernate_time = DWT_GetTimeline_ms();                                             // 记录触发指令的时间
+        dead_time = 1000;
     }
-//    last_detect_time = DWT_GetTimeline_ms();
-//    last_total_angle = loader->measure.total_angle;
 }
-
-// 热量控制
-//void Heat_control()
-//{
-//    if(shoot_cmd_recv.heat>=(0.9*shoot_cmd_recv.heat_limit))
-//    {
-//        DJIMotorStop(loader);
-//    }
-//    else
-//    {
-//        DJIMotorEnable(loader);
-//    }
-//}
-
 /* 机器人发射机构控制核心任务 */
 void ShootTask()
 {
-    //从cmd获取控制数据
+    // 从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)
     {
-        DJIMotorSetRef(friction_l, 0);
-        DJIMotorSetRef(friction_r, 0);
+//        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;
 
     // 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换
-    switch (shoot_cmd_recv.loader_mode)
+    switch (shoot_cmd_recv.load_mode)
     {
-        // 停止拨盘
-        case LOAD_STOP:
-            DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环
-            DJIMotorSetRef(loader, 0);             // 同时设定参考值为0,这样停止的速度最快
-            break;
-            // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
-        case LOAD_1_BULLET:                                                                     // 激活能量机关/干扰对方用,英雄用.
-            DJIMotorOuterLoop(loader, ANGLE_LOOP);                              // 切换到角度环
-            DJIMotorSetRef(loader, loader->measure.total_angle - ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
-            hibernate_time = DWT_GetTimeline_ms();                                              // 记录触发指令的时间
-            dead_time = 150;                                                                    // 完成1发弹丸发射的时间
-            break;
-            // 三连发,如果不需要后续可能删除
-        case LOAD_3_BULLET:
-            DJIMotorOuterLoop(loader, ANGLE_LOOP);                                     // 切换到角度环
-            DJIMotorSetRef(loader, loader->measure.total_angle - 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
-            hibernate_time = DWT_GetTimeline_ms();                                                  // 记录触发指令的时间
-            dead_time = 1000;                                                                       // 完成3发弹丸发射的时间
-            break;
-            // 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
-        case LOAD_BURSTFIRE:
-            DJIMotorOuterLoop(loader, SPEED_LOOP);
-            DJIMotorSetRef(loader, -(shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 8));
+    // 停止拨盘
+    case LOAD_STOP:
+        DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环
+        DJIMotorSetRef(loader, 0);             // 同时设定参考值为0,这样停止的速度最快
+        break;
+    // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
+    case LOAD_1_BULLET:                                                                     // 激活能量机关/干扰对方用,英雄用.
+        DJIMotorOuterLoop(loader, ANGLE_LOOP);                                              // 切换到角度环v
+        DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
+        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发
+        hibernate_time = DWT_GetTimeline_ms();                                                  // 记录触发指令的时间
+        dead_time = 300;                                                                        // 完成3发弹丸发射的时间
+        break;
+    // 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
+    case LOAD_BURSTFIRE:
+        DJIMotorOuterLoop(loader, SPEED_LOOP);
+        DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER);
+        // x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是)
+        break;
+    // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
+    // 也有可能需要从switch-case中独立出来
+    case LOAD_REVERSE:
+        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)
+            ; // 未知模式,停止运行,检查指针越界,内存溢出等问题
 
-            // x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
-            break;
-            // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
-            // 也有可能需要从switch-case中独立出来
-        case LOAD_REVERSE:
-            DJIMotorOuterLoop(loader, SPEED_LOOP);
-            DJIMotorSetRef(loader, 8 * 360 * REDUCTION_RATIO_LOADER / 8);             // 固定速度反转
-            hibernate_time = DWT_GetTimeline_ms();                                              // 记录触发指令的时间
-            dead_time = 100;                                                                    // 翻转500ms
-            shoot_feedback_data.stalled_flag = 0 ; //清除堵转标志
-            // ...
-            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, 39000);
-        DJIMotorSetRef(friction_r, 39000);
-//                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(lid,115);// 小107 大115
+        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(lid,10);
+        Servo_Motor_FreeAngle_Set(telescope,60);
     }
 
-    // 开关热控
-    if(shoot_cmd_recv.heat_mode == HEAT_OPEN)
-    {
-        if(shoot_cmd_recv.heat>=(0.85*shoot_cmd_recv.heat_limit))
-        {
-            DJIMotorStop(loader);
-        }
-        else
-        {
-            DJIMotorEnable(loader);
-        }
-    }
-    else if(shoot_cmd_recv.heat_mode == HEAT_CLOSE) {
-        DJIMotorEnable(loader);
-    }
-    //卡弹检测
-    stalled_detect();
-    // 热量控制
-    //Heat_control();
-
     // 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈
+    shoot_feedback_data.Motor_Temperature = friction_r->measure.temperature;
+    //推送消息
     PubPushMessage(shoot_pub, (void *)&shoot_feedback_data);
+
 }

application/shoot/shoot.h

@@ -1,15 +1,16 @@
 #ifndef SHOOT_H
 #define SHOOT_H
 
+
 /**
  * @brief 发射初始化,会被RobotInit()调用
- *
+ * 
  */
 void ShootInit();
 
 /**
  * @brief 发射任务
- *
+ * 
  */
 void ShootTask();
 

bsp/usb/bsp_usb.c

@@ -27,5 +27,5 @@ uint8_t *USBInit(USB_Init_Config_s usb_conf)
 
 void USBTransmit(uint8_t *buffer, uint16_t len)
 {
-    //CDC_Transmit_FS(buffer, len); // 发送
+    CDC_Transmit_FS(buffer, len); // 发送
 }

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

diary.md

@@ -0,0 +1,3 @@
+4.20：
+改pitch电机，反馈负值相反
+电机反馈速度反馈值太陡，无法改到电机反馈

modules/algorithm/crc16.c

@@ -33,7 +33,8 @@ static uint16_t crc_tab16[256] = {
  *要检查的字节也是一个参数。字符串中的字节数为
  *受恒定大小最大值的限制。
  */
-uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes) {
+uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes)
+{
 //    uint16_t crc;
 //    const uint8_t *ptr;
 //    uint16_t a;
@@ -66,7 +67,8 @@ 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 crc_modbus(const uint8_t *input_str, uint16_t num_bytes)
+{
     uint16_t crc;
     const uint8_t *ptr;
     uint16_t a;
@@ -77,9 +79,10 @@ uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes) {
     crc = CRC_START_MODBUS;
     ptr = input_str;
     if (ptr != NULL)
-        for (a = 0; a < num_bytes; a++) {
+        for (a = 0; a < num_bytes; a++)
+        {
 
-            crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t) *ptr++) & 0x00FF];
+            crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t)*ptr++) & 0x00FF];
         }
     return crc;
 }
@@ -90,10 +93,11 @@ uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes) {
  *函数update_crc_16()根据
  *前一个循环冗余校验值和下一个要检查的数据字节。
  */
-uint16_t update_crc_16(uint16_t crc, uint8_t c) {
+uint16_t update_crc_16(uint16_t crc, uint8_t c)
+{
     if (!crc_tab16_init)
         init_crc16_tab();
-    return (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t) c) & 0x00FF];
+    return (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t)c) & 0x00FF];
 }
 
 /*
@@ -104,15 +108,18 @@ uint16_t update_crc_16(uint16_t crc, uint8_t c) {
  *查找表首次由init_crc16_tab()例程计算
  *调用循环冗余校验函数。
  */
-void init_crc16_tab(void) {
+void init_crc16_tab(void)
+{
     uint16_t i;
     uint16_t j;
     uint16_t crc;
     uint16_t c;
-    for (i = 0; i < 256; i++) {
+    for (i = 0; i < 256; i++)
+    {
         crc = 0;
         c = i;
-        for (j = 0; j < 8; j++) {
+        for (j = 0; j < 8; j++)
+        {
             if ((crc ^ c) & 0x0001)
                 crc = (crc >> 1) ^ CRC_POLY_16;
             else
@@ -123,12 +130,13 @@ void init_crc16_tab(void) {
     }
     crc_tab16_init = 1;
 }
-
-uint32_t VerifyCRC16CheckSum(uint8_t *pchMessage, uint32_t dwLength) {
+uint32_t VerifyCRC16CheckSum(uint8_t *pchMessage, uint32_t dwLength)
+{
     uint16_t wExpected = 0;
-    if ((pchMessage == NULL) || (dwLength <= 2)) {
+    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]);
-}
+}

modules/algorithm/crc16.h

@@ -11,4 +11,5 @@ 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

modules/algorithm/user_lib.c

@@ -212,20 +212,6 @@ void MatInit(mat *m, uint8_t row, uint8_t col)
     m->numRows = row;
     m->pData = (float *)zmalloc(row * col * sizeof(float));
 }
-/* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */
-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);
-}
-
-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;
-}
 /**
   * @brief          一阶低通滤波初始化
   * @author         RM
@@ -234,7 +220,7 @@ float uint_to_float(int x_int, float x_min, float x_max, int bits)
   * @param[in]      滤波参数
   * @retval         返回空
   */
-void first_order_filter_init(first_order_filter_type_t *first_order_filter_type, float frame_period, const float num[1])
+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];
@@ -249,10 +235,9 @@ void first_order_filter_init(first_order_filter_type_t *first_order_filter_type,
   * @param[in]      间隔的时间，单位 s
   * @retval         返回空
   */
-void first_order_filter_cali(first_order_filter_type_t *first_order_filter_type, float input)
+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;
-}
-
+}

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
@@ -53,22 +60,6 @@ void MatInit(mat *m, uint8_t row, uint8_t col);
 #ifndef PI
 #define PI 3.14159265354f
 #endif
-typedef  struct
-{
-    float input;        //输入数据
-    float out;          //滤波输出的数据
-    float num[1];       //滤波参数
-    float frame_period; //滤波的时间间隔 单位 s
-} first_order_filter_type_t;
-
-typedef struct
-{
-    float input;        //输入数据
-    float out;          //输出数据
-    float min_value;    //限幅最小值
-    float max_value;    //限幅最大值
-    float frame_period; //时间间隔
-} ramp_function_source_t;
 
 #define VAL_LIMIT(val, min, max) \
     do                           \
@@ -136,13 +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);
-float uint_to_float(int x_int, float x_min, float x_max, int bits);
-uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits);
-//一阶低通滤波初始化
-void first_order_filter_init(first_order_filter_type_t *first_order_filter_type, float frame_period, const float num[1]);
-//一阶低通滤波计算
-void first_order_filter_cali(first_order_filter_type_t *first_order_filter_type, float input);
-
+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

modules/auto_aim/auto_aim.c

@@ -1,302 +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 = 0; // 选择的装甲板
-    // 为平衡步兵
-    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.0 * PI / 3.0);
-            else
-                tmp_yaw = aim_sel->target_state.yaw - i * (2.0 * PI / 3.0);
-            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 * cos(tmp_yaw);
-            aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * sin(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.0 * PI / 3.0);
-            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];
-        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 = 0;
-
-        for (i = 1; i < 3; i++) {
-            if (distance[i] < distance[label_first]) {
-                label_second = label_first;
-                label_first = i;
-            }  else if (distance[i] < distance[label_second] && distance[i] < distance[label_first]) {
-                label_second = i;
-            }else if (distance[i] < distance[label_second]) {
-                label_second = i;
-            }
-        }
-
-        //再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
-        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;
-
-    } else {
-        for (i = 0; i < 4; i++) {
-            float tmp_yaw = aim_sel->target_state.yaw + i * PI / 2.0;
-            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[4];
-        for (i = 0; i < 4; 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_first]){
-            label_second = label_first;
-            label_first = 2;
-        }
-        else if(distance[2]<distance[label_second])
-            label_second = 2;
-
-
-        if(distance[3]<distance[label_first]){
-            label_second = label_first;
-            label_first = 3;
-        }
-        else if(distance[3]<distance[label_second])
-            label_second = 3;
-
-        //再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
-        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]      x:瞄准时shooter_link下目标x坐标
-  * @param[in]      vx:瞄准时shooter_link下目标x速度
-  * @param[in]      v_x0:弹速水平分量
-  * @retval         返回空
-  */
-const float k1 = 0.0761; //标准大气压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 = log(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.025;//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 = 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;
-}
+//
+// 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;
+}

modules/auto_aim/auto_aim.h

@@ -1,77 +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; //预瞄时间差
-    uint8_t 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
+//
+// 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

modules/imu/ins_task.c

@@ -19,20 +19,15 @@
 #include "user_lib.h"
 #include "general_def.h"
 #include "master_process.h"
-
-#include "vofa.h"
+#include "crc16.h"
 
 static INS_t INS;
 static IMU_Param_t IMU_Param;
 static PIDInstance TempCtrl = {0};
 
-//const float xb[3] = {1, 0, 0};
-//const float yb[3] = {0, 1, 0};
-//const float zb[3] = {0, 0, 1};
-
-const float xb[3] = {0, 0, 1};
+const float xb[3] = {1, 0, 0};
 const float yb[3] = {0, 1, 0};
-const float zb[3] = {-1, 0, 0};
+const float zb[3] = {0, 0, 1};
 
 // 用于获取两次采样之间的时间间隔
 static uint32_t INS_DWT_Count = 0;
@@ -66,9 +61,9 @@ static void InitQuaternion(float *init_q4)
     for (uint8_t i = 0; i < 100; ++i)
     {
         BMI088_Read(&BMI088);
-        acc_init[X] += -BMI088.Accel[2];
+        acc_init[X] += BMI088.Accel[X];
         acc_init[Y] += BMI088.Accel[Y];
-        acc_init[Z] += BMI088.Accel[0];
+        acc_init[Z] += BMI088.Accel[Z];
         DWT_Delay(0.001);
     }
     for (uint8_t i = 0; i < 3; ++i)
@@ -78,9 +73,6 @@ static void InitQuaternion(float *init_q4)
     float angle = acosf(Dot3d(acc_init, gravity_norm));
     Cross3d(acc_init, gravity_norm, axis_rot);
     Norm3d(axis_rot);
-
-    //q = cos (a/2) + i(x * sin(a/2)) + j(y * sin(a/2)) + k(z * sin(a/2))
-    //其中a表示旋转角度，(x,y,z)表示旋转轴  计算由实际重力加速度方向(0,0,1)到当前加速度方向的旋转
     init_q4[0] = cosf(angle / 2.0f);
     for (uint8_t i = 0; i < 2; ++i)
         init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量)
@@ -106,7 +98,7 @@ attitude_t *INS_Init(void)
     IMU_Param.flag = 1;
 
     float init_quaternion[4] = {0};
-    InitQuaternion(init_quaternion);//计算由实际重力加速度方向(0,0,1)到当前加速度方向的旋转
+    InitQuaternion(init_quaternion);
     IMU_QuaternionEKF_Init(init_quaternion, 10, 0.001, 1000000, 1, 0);
     // imu heat init
     PID_Init_Config_s config = {.MaxOut = 2000,
@@ -138,12 +130,12 @@ void INS_Task(void)
     {
         BMI088_Read(&BMI088);
 
-        INS.Accel[X] = -BMI088.Accel[2];
-        INS.Accel[Y] = BMI088.Accel[1];
-        INS.Accel[Z] = BMI088.Accel[0];
-        INS.Gyro[X] = -BMI088.Gyro[2];
-        INS.Gyro[Y] = BMI088.Gyro[1];
-        INS.Gyro[Z] = BMI088.Gyro[0];
+        INS.Accel[X] = BMI088.Accel[X];
+        INS.Accel[Y] = BMI088.Accel[Y];
+        INS.Accel[Z] = BMI088.Accel[Z];
+        INS.Gyro[X] = BMI088.Gyro[X];
+        INS.Gyro[Y] = BMI088.Gyro[Y];
+        INS.Gyro[Z] = BMI088.Gyro[Z];
 
         // demo function,用于修正安装误差,可以不管,本demo暂时没用
         IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel);
@@ -167,40 +159,17 @@ 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;
         INS.Pitch = QEKF_INS.Pitch;
         INS.Roll = QEKF_INS.Roll;
-
-//        // get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)
-//        if (INS.Yaw - INS.YawAngleLast > 180.0f)
-//        {
-//            INS.YawRoundCount--;
-//        }
-//        else if (INS.Yaw - INS.YawAngleLast < -180.0f)
-//        {
-//            INS.YawRoundCount++;
-//        }
-//        INS.YawTotalAngle = 360.0f * INS.YawRoundCount + INS.Yaw;
         INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
 
-//        float vofa_send_data[9];
-//        vofa_send_data[0]=INS.Yaw;
-//        vofa_send_data[1]=INS.Pitch;
-//        vofa_send_data[2]=INS.Roll;
-//        vofa_send_data[3]=INS.Gyro[X];
-//        vofa_send_data[4]=INS.Gyro[Y];
-//        vofa_send_data[5]=INS.Gyro[Z];
-//        vofa_send_data[6]=BMI088.Accel[X];
-//        vofa_send_data[7]=BMI088.Accel[Y];
-//        vofa_send_data[8]=BMI088.Accel[Z];
-//        vofa_justfloat_output(vofa_send_data,36,&huart1);
-
-        VisionSetAltitude(INS.Yaw, INS.Pitch);
+        //VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll);
+        VisionSetAltitude(INS.Yaw * PI / 180, INS.Pitch * PI / 180);
     }
 
     // temperature control

modules/imu/ins_task.h

@@ -23,10 +23,6 @@
 #define Y 1
 #define Z 2
 
-//#define X 2
-//#define Y 1
-//#define Z 0
-
 #define INS_TASK_PERIOD 1
 
 typedef struct
@@ -38,7 +34,6 @@ typedef struct
     float Pitch;
     float Yaw;
     float YawTotalAngle;
-
 } attitude_t; // 最终解算得到的角度,以及yaw转动的总角度(方便多圈控制)
 
 typedef struct
@@ -68,9 +63,6 @@ typedef struct
     float Yaw;
     float YawTotalAngle;
 
-    float YawAngleLast;
-    float YawRoundCount;
-
     uint8_t init;
 } INS_t;
 

modules/master_machine/master_process.c

@@ -31,24 +31,18 @@ void VisionSetFlag(Enemy_Color_e enemy_color)
     send_data.reserved = 0;
 }
 
-//void VisionSetAltitude(float yaw, float pitch)
-//{
-//    send_data.yaw = yaw;
-//    send_data.pitch = pitch;
-//}
 void VisionSetAltitude(float yaw, float pitch)
 {
     send_data.yaw = yaw;
     send_data.pitch = pitch;
 }
 
-void VisionSetAim(float aim_x, float aim_y, float aim_z) {
+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()导致的死锁,使得无法
@@ -177,8 +171,8 @@ void VisionSend()
     static uint8_t send_buffer[24]={0};
 
     send_data.header = 0x5A;
-//    VisionSetFlag(data->detect_color);
-//    VisionSetAim(data->aim_x,data->aim_y,data->aim_z);
+//    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));

modules/master_machine/master_process.h

@@ -8,40 +8,45 @@
 #define VISION_SEND_SIZE 36u
 
 #pragma pack(1)
-typedef enum {
-    NO_FIRE = 0,
-    AUTO_FIRE = 1,
-    AUTO_AIM = 2
+typedef enum
+{
+	NO_FIRE = 0,
+	AUTO_FIRE = 1,
+	AUTO_AIM = 2
 } Fire_Mode_e;
 
-typedef enum {
-    NO_TARGET = 0,
-    TARGET_CONVERGING = 1,
-    READY_TO_FIRE = 2
+typedef enum
+{
+	NO_TARGET = 0,
+	TARGET_CONVERGING = 1,
+	READY_TO_FIRE = 2
 } Target_State_e;
 
-typedef enum {
-    NO_TARGET_NUM = 0,
-    HERO1 = 1,
-    ENGINEER2 = 2,
-    INFANTRY3 = 3,
-    INFANTRY4 = 4,
-    INFANTRY5 = 5,
-    OUTPOST = 6,
-    SENTRY = 7,
-    BASE = 8
+typedef enum
+{
+	NO_TARGET_NUM = 0,
+	HERO1 = 1,
+	ENGINEER2 = 2,
+	INFANTRY3 = 3,
+	INFANTRY4 = 4,
+	INFANTRY5 = 5,
+	OUTPOST = 6,
+	SENTRY = 7,
+	BASE = 8
 } Target_Type_e;
 
-typedef struct {
-    Fire_Mode_e fire_mode;
-    Target_State_e target_state;
-    Target_Type_e target_type;
+typedef struct
+{
+	Fire_Mode_e fire_mode;
+	Target_State_e target_state;
+	Target_Type_e target_type;
 
-    float pitch;
-    float yaw;
+	float pitch;
+	float yaw;
 } Vision_Recv_s;
 
-typedef enum {
+typedef enum
+{
 //	COLOR_NONE = 0,
 //	COLOR_BLUE = 1,
 //	COLOR_RED = 2,
@@ -49,32 +54,26 @@ typedef enum {
     ENEMY_COLOR_BLUE = 1,
 } Enemy_Color_e;
 
-typedef enum {
-    VISION_MODE_AIM = 0,
-    VISION_MODE_SMALL_BUFF = 1,
-    VISION_MODE_BIG_BUFF = 2
+typedef enum
+{
+	VISION_MODE_AIM = 0,
+	VISION_MODE_SMALL_BUFF = 1,
+	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;
+typedef struct
+{
+	Enemy_Color_e enemy_color;
+	Work_Mode_e work_mode;
+
+	float yaw;
+	float pitch;
+	float roll;
 } Vision_Send_s;
 
-typedef  struct {
+typedef __packed struct {
     uint8_t header;//0x5A
     uint8_t detect_color: 1;
     uint8_t reserved: 7;
@@ -86,7 +85,8 @@ typedef  struct {
     uint16_t checksum;
 } SendPacket_t;
 
-typedef  struct {
+typedef __packed struct 
+{
     uint8_t header; //= 0xA5;
     uint8_t tracking: 1;
     uint8_t id: 3;          // 0-outpost 6-guard 7-base
@@ -105,6 +105,7 @@ typedef  struct {
     float dz;
     uint16_t checksum;
 } RecievePacket_t;
+
 #pragma pack()
 
 /**
@@ -130,7 +131,6 @@ void VisionSend();
  */
 //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 设置发送数据的姿态部分
  *

modules/motor/DJImotor/dji_motor.c

@@ -2,7 +2,8 @@
 #include "general_def.h"
 #include "bsp_dwt.h"
 #include "bsp_log.h"
-#include "user_lib.h"
+#include "power_meter/power_meter.h"
+
 
 static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
 /* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
@@ -20,121 +21,97 @@ static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL}; // 会在co
  * can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF
  * can2: [3]:0x1FF,[4]:0x200,[5]:0x2FF
  */
-static CANInstance sender_assignment[10] = {
-        [0] = {.can_handle = &hcan1, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
-                0}},
-        [1] = {.can_handle = &hcan1, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
-                0}},
-        [2] = {.can_handle = &hcan1, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
-                0}},
-        [3] = {.can_handle = &hcan2, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
-                0}},
-        [4] = {.can_handle = &hcan2, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
-                0}},
-        [5] = {.can_handle = &hcan2, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
-                0}},
-        [6] = {.can_handle = &hcan1, .txconf.StdId = 0x1fe, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
-                0}},
-        [7] = {.can_handle = &hcan1, .txconf.StdId = 0x2fe, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
-                0}},
-        [8] = {.can_handle = &hcan2, .txconf.StdId = 0x1fe, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
-                0}},
-        [9] = {.can_handle = &hcan2, .txconf.StdId = 0x2fe, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
-                0}}
+static CANInstance sender_assignment[6] = {
+    [0] = {.can_handle = &hcan1, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
+    [1] = {.can_handle = &hcan1, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
+    [2] = {.can_handle = &hcan1, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
+    [3] = {.can_handle = &hcan2, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
+    [4] = {.can_handle = &hcan2, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
+    [5] = {.can_handle = &hcan2, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
 };
 
 /**
  * @brief 10个用于确认是否有电机注册到sender_assignment中的标志位,防止发送空帧,此变量将在DJIMotorControl()使用
  *        flag的初始化在 MotorSenderGrouping()中进行
  */
-static uint8_t sender_enable_flag[10] = {0};
+static uint8_t sender_enable_flag[6] = {0};
 
 /**
  * @brief 根据电调/拨码开关上的ID,根据说明书的默认id分配方式计算发送ID和接收ID,
  *        并对电机进行分组以便处理多电机控制命令
  */
-static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *config) {
+static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *config)
+{
     uint8_t motor_id = config->tx_id - 1; // 下标从零开始,先减一方便赋值
     uint8_t motor_send_num;
     uint8_t motor_grouping;
 
-    switch (motor->motor_type) {
-        case M2006:
-        case M3508:
-            if (motor_id < 4) // 根据ID分组
+    switch (motor->motor_type)
+    {
+    case M2006:
+    case M3508:
+        if (motor_id < 4) // 根据ID分组
+        {
+            motor_send_num = motor_id;
+            motor_grouping = config->can_handle == &hcan1 ? 1 : 4;
+        }
+        else
+        {
+            motor_send_num = motor_id - 4;
+            motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
+        }
+
+        // 计算接收id并设置分组发送id
+        config->rx_id = 0x200 + motor_id + 1;   // 把ID+1,进行分组设置
+        sender_enable_flag[motor_grouping] = 1; // 设置发送标志位,防止发送空帧
+        motor->message_num = motor_send_num;
+        motor->sender_group = motor_grouping;
+
+        // 检查是否发生id冲突
+        for (size_t i = 0; i < idx; ++i)
+        {
+            if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
             {
-                motor_send_num = motor_id;
-                motor_grouping = config->can_handle == &hcan1 ? 1 : 4;
-            } else {
-                motor_send_num = motor_id - 4;
-                motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
+                LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
+                uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
+                while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
+                    LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
             }
+        }
+        break;
 
-            // 计算接收id并设置分组发送id
-            config->rx_id = 0x200 + motor_id + 1;   // 把ID+1,进行分组设置
-            sender_enable_flag[motor_grouping] = 1; // 设置发送标志位,防止发送空帧
-            motor->message_num = motor_send_num;
-            motor->sender_group = motor_grouping;
+    case GM6020:
+        if (motor_id < 4)
+        {
+            motor_send_num = motor_id;
+            motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
+        }
+        else
+        {
+            motor_send_num = motor_id - 4;
+            motor_grouping = config->can_handle == &hcan1 ? 2 : 5;
+        }
 
-            // 检查是否发生id冲突
-            for (size_t i = 0; i < idx; ++i) {
-                if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle &&
-                    dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id) {
-                    LOGERROR(
-                            "[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
-                    uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
-                    while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
-                        LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
-                }
-            }
-            break;
+        config->rx_id = 0x204 + motor_id + 1;   // 把ID+1,进行分组设置
+        sender_enable_flag[motor_grouping] = 1; // 只要有电机注册到这个分组,置为1;在发送函数中会通过此标志判断是否有电机注册
+        motor->message_num = motor_send_num;
+        motor->sender_group = motor_grouping;
 
-        case GM6020:
-            if(motor->motor_control_type == CURRENT_CONTROL) //6020电流控制帧id  0x1fe  0x2fe
+        for (size_t i = 0; i < idx; ++i)
+        {
+            if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
             {
-                if (motor_id < 4)
-                {
-                    motor_send_num = motor_id;
-                    motor_grouping = config->can_handle == &hcan1 ? 6 : 8;
-                }
-                else
-                {
-                    motor_send_num = motor_id - 4;
-                    motor_grouping = config->can_handle == &hcan1 ? 7 : 9;
-                }
-            }else{
-                if (motor_id < 4)
-                {
-                    motor_send_num = motor_id;
-                    motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
-                }
-                else
-                {
-                    motor_send_num = motor_id - 4;
-                    motor_grouping = config->can_handle == &hcan1 ? 2 : 5;
-                }
+                LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
+                uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
+                while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
+                    LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
             }
+        }
+        break;
 
-            config->rx_id = 0x204 + motor_id + 1;   // 把ID+1,进行分组设置
-            sender_enable_flag[motor_grouping] = 1; // 只要有电机注册到这个分组,置为1;在发送函数中会通过此标志判断是否有电机注册
-            motor->message_num = motor_send_num;
-            motor->sender_group = motor_grouping;
-
-            for (size_t i = 0; i < idx; ++i) {
-                if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle &&
-                    dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id) {
-                    LOGERROR(
-                            "[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
-                    uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
-                    while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
-                        LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
-                }
-            }
-            break;
-
-        default: // other motors should not be registered here
-            while (1)
-                LOGERROR("[dji_motor]You must not register other motors using the API of DJI motor."); // 其他电机不应该在这里注册
+    default: // other motors should not be registered here
+        while (1)
+            LOGERROR("[dji_motor]You must not register other motors using the API of DJI motor."); // 其他电机不应该在这里注册
     }
 }
 
@@ -144,11 +121,12 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
  *
  * @param _instance 收到数据的instance,通过遍历与所有电机进行对比以选择正确的实例
  */
-static void DecodeDJIMotor(CANInstance *_instance) {
+static void DecodeDJIMotor(CANInstance *_instance)
+{
     // 这里对can instance的id进行了强制转换,从而获得电机的instance实例地址
     // _instance指针指向的id是对应电机instance的地址,通过强制转换为电机instance的指针,再通过->运算符访问电机的成员motor_measure,最后取地址获得指针
     uint8_t *rxbuff = _instance->rx_buff;
-    DJIMotorInstance *motor = (DJIMotorInstance *) _instance->id;
+    DJIMotorInstance *motor = (DJIMotorInstance *)_instance->id;
     DJI_Motor_Measure_s *measure = &motor->measure; // measure要多次使用,保存指针减小访存开销
 
     DaemonReload(motor->daemon);
@@ -156,12 +134,12 @@ static void DecodeDJIMotor(CANInstance *_instance) {
 
     // 解析数据并对电流和速度进行滤波,电机的反馈报文具体格式见电机说明手册
     measure->last_ecd = measure->ecd;
-    measure->ecd = ((uint16_t) rxbuff[0]) << 8 | rxbuff[1];
-    measure->angle_single_round = ECD_ANGLE_COEF_DJI * (float) measure->ecd;
+    measure->ecd = ((uint16_t)rxbuff[0]) << 8 | rxbuff[1];
+    measure->angle_single_round = ECD_ANGLE_COEF_DJI * (float)measure->ecd;
     measure->speed_aps = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_aps +
-                         RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * (float) ((int16_t) (rxbuff[2] << 8 | rxbuff[3]));
+                         RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * (float)((int16_t)(rxbuff[2] << 8 | rxbuff[3]));
     measure->real_current = (1.0f - CURRENT_SMOOTH_COEF) * measure->real_current +
-                            CURRENT_SMOOTH_COEF * (float) ((int16_t) (rxbuff[4] << 8 | rxbuff[5]));
+                            CURRENT_SMOOTH_COEF * (float)((int16_t)(rxbuff[4] << 8 | rxbuff[5]));
     measure->temperature = rxbuff[6];
 
     // 多圈角度计算,前提是假设两次采样间电机转过的角度小于180°,自己画个图就清楚计算过程了
@@ -172,24 +150,24 @@ static void DecodeDJIMotor(CANInstance *_instance) {
     measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
 }
 
-static void DJIMotorLostCallback(void *motor_ptr) {
-    DJIMotorInstance *motor = (DJIMotorInstance *) motor_ptr;
+static void DJIMotorLostCallback(void *motor_ptr)
+{
+    DJIMotorInstance *motor = (DJIMotorInstance *)motor_ptr;
     uint16_t can_bus = motor->motor_can_instance->can_handle == &hcan1 ? 1 : 2;
     LOGWARNING("[dji_motor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instance->tx_id);
 }
 
 // 电机初始化,返回一个电机实例
-DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config) {
-    DJIMotorInstance *instance = (DJIMotorInstance *) malloc(sizeof(DJIMotorInstance));
+DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
+{
+    DJIMotorInstance *instance = (DJIMotorInstance *)malloc(sizeof(DJIMotorInstance));
     memset(instance, 0, sizeof(DJIMotorInstance));
 
     // motor basic setting 电机基本设置
     instance->motor_type = config->motor_type;                         // 6020 or 2006 or 3508
     instance->motor_settings = config->controller_setting_init_config; // 正反转,闭环类型等
-    instance->motor_control_type = config->motor_control_type; //电流控制or电压控制
 
     // 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;
@@ -208,9 +186,9 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config) {
 
     // 注册守护线程
     Daemon_Init_Config_s daemon_config = {
-            .callback = DJIMotorLostCallback,
-            .owner_id = instance,
-            .reload_count = 100, // 20ms未收到数据则丢失
+        .callback = DJIMotorLostCallback,
+        .owner_id = instance,
+        .reload_count = 2, // 20ms未收到数据则丢失
     };
     instance->daemon = DaemonRegister(&daemon_config);
 
@@ -220,7 +198,8 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config) {
 }
 
 /* 电流只能通过电机自带传感器监测,后续考虑加入力矩传感器应变片等 */
-void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback_Source_e type) {
+void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback_Source_e type)
+{
     if (loop == ANGLE_LOOP)
         motor->motor_settings.angle_feedback_source = type;
     else if (loop == SPEED_LOOP)
@@ -229,24 +208,28 @@ void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback
         LOGERROR("[dji_motor] loop type error, check memory access and func param"); // 检查是否传入了正确的LOOP类型,或发生了指针越界
 }
 
-void DJIMotorStop(DJIMotorInstance *motor) {
+void DJIMotorStop(DJIMotorInstance *motor)
+{
     motor->stop_flag = MOTOR_STOP;
 }
 
-void DJIMotorEnable(DJIMotorInstance *motor) {
+void DJIMotorEnable(DJIMotorInstance *motor)
+{
     motor->stop_flag = MOTOR_ENALBED;
 }
 
 /* 修改电机的实际闭环对象 */
-void DJIMotorOuterLoop(DJIMotorInstance *motor, Closeloop_Type_e outer_loop) {
+void DJIMotorOuterLoop(DJIMotorInstance *motor, Closeloop_Type_e outer_loop)
+{
     motor->motor_settings.outer_loop_type = outer_loop;
 }
 
 // 设置参考值
-void DJIMotorSetRef(DJIMotorInstance *motor, float ref) {
+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)
@@ -257,10 +240,9 @@ static float EstimatePower(DJIMotorInstance* chassis_motor,float output)
     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() {
+void DJIMotorControl()
+{
     // 直接保存一次指针引用从而减小访存的开销,同样可以提高可读性
     uint8_t group, num; // 电机组号和组内编号
     int16_t set;        // 电机控制CAN发送设定值
@@ -271,7 +253,8 @@ void DJIMotorControl() {
     float pid_measure, pid_ref;             // 电机PID测量值和设定值
 
     // 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
-    for (size_t i = 0; i < idx; ++i) { // 减小访存开销,先保存指针引用
+    for (size_t i = 0; i < idx; ++i)
+    { // 减小访存开销,先保存指针引用
         motor = dji_motor_instance[i];
         motor_setting = &motor->motor_settings;
         motor_controller = &motor->motor_controller;
@@ -282,7 +265,8 @@ void DJIMotorControl() {
 
         // pid_ref会顺次通过被启用的闭环充当数据的载体
         // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
-        if ((motor_setting->close_loop_type & ANGLE_LOOP) && (motor_setting->outer_loop_type == ANGLE_LOOP)) {
+        if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP)
+        {
             if (motor_setting->angle_feedback_source == OTHER_FEED)
                 pid_measure = *motor_controller->other_angle_feedback_ptr;
             else
@@ -292,21 +276,16 @@ void DJIMotorControl() {
         }
 
         // 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
-        if ((motor_setting->close_loop_type & SPEED_LOOP) &&
-            (motor_setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))) {
+
+
+        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
-            {
-                if(motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
-                    pid_measure = - measure->speed_aps;
-                else //NORMAL
-                    pid_measure = measure->speed_aps;
-            }
-
+                pid_measure = measure->speed_aps;
             // 更新pid_ref进入下一个环
             pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref);
         }
@@ -314,9 +293,9 @@ void DJIMotorControl() {
         // 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
         if (motor_setting->feedforward_flag & CURRENT_FEEDFORWARD)
             pid_ref += *motor_controller->current_feedforward_ptr;
-        if (motor_setting->close_loop_type & CURRENT_LOOP) {
-            //现在电调都有内置电流环，无需pid计算
-            //pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
+        if (motor_setting->close_loop_type & CURRENT_LOOP)
+        {
+            pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
         }
 
         if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
@@ -327,11 +306,9 @@ void DJIMotorControl() {
         {
             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;
-            //这里K应该使用所有底盘电机一起计算 (在底盘任务中)
-            //float K = motor_controller->motor_power_max / motor_controller->motor_power_predict;
+            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)//未超过最大功率 或做负功的电机 不做限制
+            if(K>=1 || motor_controller->motor_power_predict < 0)//未超过最大功率时 或做负功的电机 不做限制
             {
 
             }
@@ -341,27 +318,30 @@ void DJIMotorControl() {
 
                 float a = motor_power_K[2] ;
                 float b = motor_power_K[0] * w;
-                float c = motor_power_K[1] * w * w - P_cmd;
+                float c = motor_power_K[1] * w * w - P_cmd +1.021f;
 
                 if(pid_ref < 0)
-                    pid_ref = (-b - sqrtf(b*b-4*a*c))/(2*a);
+                {
+                    float temp = (-b - sqrtf(b*b-4*a*c))/(2*a);
+                    if(temp < -28000) pid_ref = -28000;
+                    else pid_ref = temp;
+                }
                 else
-                    pid_ref = (-b + sqrtf(b*b-4*a*c))/(2*a);
+                {
+                    float temp = (-b + sqrtf(b*b-4*a*c))/(2*a);
+                    if(temp > 28000) pid_ref = 28000;
+                    else pid_ref = temp;
+                }
             }
-            //if( motor_controller->motor_power_predict < )
         }
-
-        // 获取最终输出  此处注意set不要超过int16_t能表达的最大数 +-32767
-
-        pid_ref = float_constrain(pid_ref,-16384,16384);
         // 获取最终输出
-        set = (int16_t) pid_ref;
+        set = (int16_t)pid_ref;
 
         // 分组填入发送数据
         group = motor->sender_group;
         num = motor->message_num;
-        sender_assignment[group].tx_buff[2 * num] = (uint8_t) (set >> 8);         // 低八位
-        sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t) (set & 0x00ff); // 高八位
+        sender_assignment[group].tx_buff[2 * num] = (uint8_t)(set >> 8);         // 低八位
+        sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t)(set & 0x00ff); // 高八位
 
         // 若该电机处于停止状态,直接将buff置零
         if (motor->stop_flag == MOTOR_STOP)
@@ -369,8 +349,10 @@ void DJIMotorControl() {
     }
 
     // 遍历flag,检查是否要发送这一帧报文
-    for (size_t i = 0; i < 10; ++i) {
-        if (sender_enable_flag[i]) {
+    for (size_t i = 0; i < 6; ++i)
+    {
+        if (sender_enable_flag[i])
+        {
             CANTransmit(&sender_assignment[i], 1);
         }
     }

modules/motor/DJImotor/dji_motor.h

@@ -58,7 +58,6 @@ typedef struct
     uint8_t message_num;
 
     Motor_Type_e motor_type;        // 电机类型
-    Motor_Control_Type_e motor_control_type;//电机控制方式
     Motor_Working_Type_e stop_flag; // 启停标志
 
     DaemonInstance* daemon;

modules/motor/DMmotor/dm4310.c

@@ -1,310 +1,230 @@
-#include "dmmotor.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值进行映射的函数,在设定发送值和解析反馈值时使用 */
-
-
-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);
-    //0 失能  1 使能  8 超压 9 欠压  A 过流  B MOS过压  C 电机线圈过温   D 通信丢失  E 过载
-    tmp =  (uint8_t)(rxbuff[0]>>4);
-    measure->state = tmp;
-    if(measure->state){
-        DaemonReload(motor->motor_daemon);
-    }
-
-    measure->last_position = measure->position;
-
-    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);
-    measure->angle_single_round =  (measure->position);
-
-    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 > PI) //3.1425926
-        measure->total_round--;
-    else if (measure->position - measure->last_position < -PI)
-        measure->total_round++;
-
-    //measure->total_angle = measure->total_round * (DM_P_MAX * DEGREE_2_RAD) + measure->angle_single_round;
-    measure->total_angle = measure->total_round * (DM_P_MAX*2) + 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);
-
-    DMMotorEnable(motor);
-    DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
-    LOGWARNING("[dm_motor] Tring to restart motor, can bus [%d] , id [%d]\"", can_bus, motor->motor_can_instance->tx_id);
-}
-void DMMotorCaliEncoder(DMMotorInstance *motor)
-{
-    DMMotorSetMode(DM_CMD_RESET_MODE, motor);
-    DWT_Delay(0.1);
-
-    DMMotorSetMode(DM_CMD_ZERO_POSITION, motor);
-    DWT_Delay(0.1);
-
-    DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
-    DWT_Delay(0.1);
-}
-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;
-
-    motor->motor_controller.current_feedforward_ptr = config->controller_param_init_config.current_feedforward_ptr;
-    motor->motor_controller.speed_feedforward_ptr = config->controller_param_init_config.speed_feedforward_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->position; // 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->feedforward_flag & CURRENT_FEEDFORWARD)
-            pid_ref += *motor_controller->current_feedforward_ptr;
-
-        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);
-        osDelay(5);
-    }
-}
-
-
-void DMMotorControl(){
-    DMMotorInstance *motor;
-    Motor_Controller_s *motor_controller;   // 电机控制器
-    DM_Motor_Measure_s *measure;
-    Motor_Control_Setting_s *setting;
-    //CANInstance *motor_can = motor->motor_can_instance;
-    //uint16_t tmp;
-    DMMotor_Send_s motor_send_mailbox;
-    float  pid_ref, set, pid_measure;
-
-    for (size_t i = 0; i < idx; ++i)
-    {
-        motor = dm_motor_instance[i];
-        setting = &motor->motor_settings;
-        motor_controller = &motor->motor_controller;
-        measure = &motor->measure;
-
-        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->position; // 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->feedforward_flag & CURRENT_FEEDFORWARD)
-            pid_ref += *motor_controller->current_feedforward_ptr;
-
-        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);
-    }
-
-}
+#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]);
+    }
+}

modules/motor/DMmotor/dm4310.h

@@ -1,76 +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 6
-
-#define DM_P_MIN  (-12.56637)//(-3.1415926f)
-#define DM_P_MAX  12.56637//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();
-void DMMotorControl();
+#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

modules/motor/ECmotor/ECA8210.c

@@ -1,179 +0,0 @@
-//
-// Created by SJQ on 2023/12/11.
-//
-
-#include "ECA8210.h"
-
-ECMotor_Report_t ECMotor_Report;
-ECMotor_SpdCMD_t ECMotor_SpdCMD;
-ECMotor_PosCMD_t ECMotor_PosCMD;
-static uint8_t idx;
-static ECMotorInstance *ecmotor_instance[EC_MOTOR_MX_CNT] = {NULL};
-
-static void swap ( uint8_t *a ,uint8_t *b )
-{
-    uint8_t temp = *a;
-    *a = *b;
-    *b = temp;
-}
-/**
- * @brief 电机反馈报文解析
- *
- * @param _instance 发生中断的caninstance
- */
-static void ECMotorDecode(CANInstance *_instance)
-{
-    ECMotorInstance *motor = (ECMotorInstance *)_instance->id; // 通过caninstance保存的father id获取对应的motorinstance
-    ECMotor_Measure_t *measure = &motor->measure;
-    uint8_t *rx_buff = _instance->rx_buff;
-
-    swap(rx_buff+1,rx_buff+2);// 输出端位置  高位在前
-
-    memcpy(&ECMotor_Report,rx_buff,sizeof(ECMotor_Report));
-    //高位在前  这两个12位数据需要做特殊处理
-    ECMotor_Report.speed_raw = (uint16_t)(rx_buff[3]<<4|(rx_buff[4]>>4));
-    ECMotor_Report.current_raw = (uint16_t)(rx_buff[4]<<8|(rx_buff[5]));
-
-    DaemonReload(motor->daemon); // 喂狗
-    measure->feed_dt = DWT_GetDeltaT(&measure->feed_dwt_cnt);
-
-    measure->angle_single_round = (float)(ECMotor_Report.angle_raw-32768) * 0.0003814697;
-    measure->speed_rads = (float)(ECMotor_Report.speed_raw-2048) * 0.008789;
-    measure->real_current = (float)(ECMotor_Report.current_raw-2048) * 0.014648;
-    measure->temperature = (ECMotor_Report.temperature_raw - 50) /2;
-}
-
-static void ECMotorLostCallback(void *motor_ptr)
-{
-    ECMotorInstance *motor = (ECMotorInstance *)motor_ptr;
-    LOGWARNING("[ECMotor] motor lost, id: %d", motor->motor_can_ins->tx_id);
-}
-
-ECMotorInstance *ECMotorInit(Motor_Init_Config_s *config)
-{
-    ECMotorInstance *motor = (ECMotorInstance *)malloc(sizeof(ECMotorInstance));
-    motor = (ECMotorInstance *)malloc(sizeof(ECMotorInstance));
-    memset(motor, 0, sizeof(ECMotorInstance));
-
-    motor->motor_settings = config->controller_setting_init_config;
-    PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID);
-    PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_PID);
-    PIDInit(&motor->angle_PID, &config->controller_param_init_config.angle_PID);
-    motor->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
-    motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
-
-    config->can_init_config.id = motor;
-    config->can_init_config.can_module_callback = ECMotorDecode;
-    config->can_init_config.rx_id = config->can_init_config.tx_id;
-    config->can_init_config.tx_id = config->can_init_config.tx_id;
-    motor->motor_can_ins = CANRegister(&config->can_init_config);
-
-    ECMotorEnable(motor);
-
-    DWT_GetDeltaT(&motor->measure.feed_dwt_cnt);
-    ecmotor_instance[idx++] = motor;
-
-    Daemon_Init_Config_s daemon_config = {
-            .callback = ECMotorLostCallback,
-            .owner_id = motor,
-            .reload_count = 5, // 50ms
-    };
-    motor->daemon = DaemonRegister(&daemon_config);
-
-    return motor;
-}
-
-
-
-void ECMotorControl()
-{
-    float pid_measure, pid_ref;
-    int16_t set;
-    ECMotorInstance *motor;
-    ECMotor_Measure_t *measure;
-    Motor_Control_Setting_s *setting;
-
-    for (size_t i = 0; i < idx; ++i)
-    {
-        motor = ecmotor_instance[i];
-        measure = &motor->measure;
-        setting = &motor->motor_settings;
-        pid_ref = motor->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->other_angle_feedback_ptr;
-            else
-                pid_measure = measure->real_current;
-            pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
-            if (setting->feedforward_flag & SPEED_FEEDFORWARD)
-                pid_ref += *motor->speed_feedforward_ptr;
-        }
-
-        if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))
-        {
-            if (setting->angle_feedback_source == OTHER_FEED)
-                pid_measure = *motor->other_speed_feedback_ptr;
-            else
-                pid_measure = measure->speed_rads;
-            pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
-            if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
-                pid_ref += *motor->current_feedforward_ptr;
-        }
-
-        if (setting->close_loop_type & CURRENT_LOOP)
-        {
-            pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
-        }
-
-        set = pid_ref;
-        if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
-            set *= -1;
-
-        ECMotor_SpdCMD.mode = 0b010;
-        ECMotor_SpdCMD.reserve = 0b000;
-        ECMotor_SpdCMD.ack_type = 0b01;
-        ECMotor_SpdCMD.spd_target = set;
-        ECMotor_SpdCMD.max_current = 10;
-        // 这里随便写的,为了兼容多电机命令.后续应该将tx_id以更好的方式表达电机id,单独使用一个CANInstance,而不是用第一个电机的CANInstance
-        //memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, &set, sizeof(uint16_t));
-        CANSetDLC(motor->motor_can_ins,7);
-        memcpy(motor->motor_can_ins->tx_buff,&ECMotor_SpdCMD,sizeof(ECMotor_SpdCMD));
-        //发送数据高位在前！！ 交换一下顺序
-        swap(motor->motor_can_ins->tx_buff+1,motor->motor_can_ins->tx_buff+4);
-        swap(motor->motor_can_ins->tx_buff+2,motor->motor_can_ins->tx_buff+3);
-        swap(motor->motor_can_ins->tx_buff+5,motor->motor_can_ins->tx_buff+6);
-
-        //memcpy(motor->motor_can_ins->tx_buff+1,&ECMotor_SpdCMD.spd_target,sizeof(float32_t));
-
-        if (motor->stop_flag == MOTOR_STOP)
-        { // 若该电机处于停止状态,直接将发送buff置零
-            memcpy(motor->motor_can_ins->tx_buff,0,sizeof(ECMotor_SpdCMD));
-        }
-
-        CANTransmit(motor->motor_can_ins,0.2);
-    }
-
-
-}
-
-void ECMotorStop(ECMotorInstance *motor)
-{
-    motor->stop_flag = MOTOR_STOP;
-}
-
-void ECMotorEnable(ECMotorInstance *motor)
-{
-    motor->stop_flag = MOTOR_ENALBED;
-}
-
-void ECMotorSetRef(ECMotorInstance *motor, float ref)
-{
-    motor->pid_ref = ref;
-}
-
-uint8_t ECMotorIsOnline(ECMotorInstance *motor)
-{
-    return DaemonIsOnline(motor->daemon);
-}

modules/motor/ECmotor/ECA8210.h

@@ -1,130 +0,0 @@
-//
-// Created by SJQ on 2023/12/11.
-//
-
-#ifndef BASIC_FRAMEWORK_ECA8210_H
-#define BASIC_FRAMEWORK_ECA8210_H
-
-#include "stdint.h"
-#include "bsp_can.h"
-#include "controller.h"
-#include "motor_def.h"
-#include "daemon.h"
-
-#define EC_MOTOR_MX_CNT 4
-
-#pragma pack(1)
-typedef  struct
-{
-    uint8_t bug_info:5;       //错误信息
-    uint8_t report_type:3;    //报文类别
-
-    uint16_t angle_raw:16;    //输出端位置数值范围：0~65536 对应-12.5f~12.5f，单位 rad
-    uint16_t speed_raw:12;    //输出端转速数值范围：0~4095 对应-18.0f~18.0f，单位 rad/s
-    uint16_t current_raw:12;  //实际电流：0~4095 对应-30~30A
-    uint8_t temperature_raw:8;//电机温度：反馈的数值数据类型为无符号 8 位，数值等于实际温度*2+50
-}ECMotor_Report_t;
-#pragma pack()
-typedef  struct
-{
-    uint8_t mode:3;    //控制模式
-    float32_t pos_target;       //错误信息
-    uint16_t spd_target:15;    //输出端位置数值范围：0~65536 对应-12.5f~12.5f，单位 rad
-    uint16_t max_current:12;    //输出端转速数值范围：0~4095 对应-18.0f~18.0f，单位 rad/s
-    uint8_t ack_type:2;  //实际电流：0~4095 对应-30~30A
-
-}ECMotor_PosCMD_t;
-#pragma pack(1)
-typedef  struct
-{
-    uint8_t ack_type:2;
-    uint8_t reserve:3;
-    uint8_t mode:3;    //控制模式
-
-
-    float32_t spd_target;    //输出端速度指令  单位RPM
-    uint16_t max_current;    //电流限幅  电机电流阈值数值解释：0~65536 对应 0~6553.6A，比例为10
-
-}ECMotor_SpdCMD_t;
-
-#pragma pack()
-
-typedef struct // EC-A8120
-{
-    uint16_t last_ecd;        // 上一次读取的编码器值
-    uint16_t ecd;             // 当前编码器值
-    float angle_single_round; // 单圈角度
-    float speed_rads;         // speed rad/s
-    float real_current;     // 实际电流
-    uint8_t temperature;      // 温度,C°
-
-    float total_angle;   // 总角度
-    int32_t total_round; // 总圈数
-
-    float feed_dt;
-    uint32_t feed_dwt_cnt;
-} ECMotor_Measure_t;
-
-typedef struct
-{
-    ECMotor_Measure_t measure;
-
-    Motor_Control_Setting_s motor_settings;
-
-    float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针
-    float *other_speed_feedback_ptr;
-    float *speed_feedforward_ptr;   // 速度前馈数据指针,可以通过此指针设置速度前馈值,或LQR等时作为速度状态变量的输入
-    float *current_feedforward_ptr; // 电流前馈指针
-    PIDInstance current_PID;
-    PIDInstance speed_PID;
-    PIDInstance angle_PID;
-    float pid_ref;
-
-    Motor_Working_Type_e stop_flag; // 启停标志
-
-    CANInstance *motor_can_ins;
-
-    DaemonInstance *daemon;
-
-} ECMotorInstance;
-
-/**
- * @brief 初始化LK电机
- *
- * @param config 电机配置
- * @return LKMotorInstance* 返回实例指针
- */
-ECMotorInstance *ECMotorInit(Motor_Init_Config_s *config);
-
-/**
- * @brief 设置参考值
- * @attention 注意此函数设定的ref是最外层闭环的输入,若要设定内层闭环的值请通过前馈数据指针设置
- *
- * @param motor 要设置的电机
- * @param ref 设定值
- */
-void ECMotorSetRef(ECMotorInstance *motor, float ref);
-
-/**
- * @brief 为所有EC电机计算pid/反转/模式控制,并通过bspcan发送电流值(发送CAN报文)
- *
- */
-void ECMotorControl();
-
-/**
- * @brief 停止EC电机,之后电机不会响应任何指令
- *
- * @param motor
- */
-void ECMotorStop(ECMotorInstance *motor);
-
-/**
- * @brief 启动EC电机
- *
- * @param motor
- */
-void ECMotorEnable(ECMotorInstance *motor);
-
-uint8_t ECMotorIsOnline(ECMotorInstance *motor);
-
-#endif //BASIC_FRAMEWORK_ECA8210_H

modules/motor/HTmotor/HT04.c

@@ -26,8 +26,19 @@ static void HTMotorSetMode(HTMotor_Mode_t cmd, HTMotorInstance *motor)
     CANTransmit(motor->motor_can_instace, 1);
     memcpy(motor->motor_can_instace->tx_buff, zero_buff, 6);
 }
-
-
+/* 两个用于将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;
+}
 
 /**
  * @brief 解析电机反馈值
@@ -105,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 = {

modules/motor/motor_def.h

@@ -67,13 +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
 {
@@ -85,18 +83,8 @@ typedef struct
     Feedback_Source_e speed_feedback_source;       // 速度反馈类型
     Feedfoward_Type_e feedforward_flag;            // 前馈标志
     Power_Limit_Type_e power_limit_flag;            //功率限制标志
-
 } Motor_Control_Setting_s;
 
-/* 电机控制方式枚举 */
-typedef enum
-{
-    CONTROL_TYPE_NONE = 0,
-    CURRENT_CONTROL,
-    VOLTAGE_CONTROL,
-} Motor_Control_Type_e;
-
-
 /* 电机控制器,包括其他来源的反馈数据指针,3环控制器和电机的参考输入*/
 // 后续增加前馈数据指针
 typedef struct
@@ -121,13 +109,12 @@ typedef struct
 typedef enum
 {
     MOTOR_TYPE_NONE = 0,
+    DM4310,
     GM6020,
     M3508,
     M2006,
     LK9025,
     HT04,
-    DM4310,
-    DM6006,
 } Motor_Type_e;
 
 /**
@@ -155,7 +142,6 @@ typedef struct
     Motor_Control_Setting_s controller_setting_init_config;
     Motor_Type_e motor_type;
     CAN_Init_Config_s can_init_config;
-    Motor_Control_Type_e motor_control_type;
 } Motor_Init_Config_s;
 
 #endif // !MOTOR_DEF_H

modules/motor/motor_task.c

@@ -1,11 +1,9 @@
 #include "motor_task.h"
 #include "LK9025.h"
-#include "ECA8210.h"
 #include "HT04.h"
 #include "dji_motor.h"
 #include "step_motor.h"
 #include "servo_motor.h"
-#include "dmmotor.h"
 
 void MotorControlTask()
 {
@@ -15,9 +13,8 @@ void MotorControlTask()
     DJIMotorControl();
 
     /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */
-    LKMotorControl();
-    ECMotorControl();
-    //DMMotorControl();
+//    LKMotorControl();
+
     // legacy support
     // 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞
     // 为了保证高频率控制,HTMotor中提供了以任务方式启动控制的接口,可通过宏定义切换

modules/referee/referee_protocol.h

@@ -65,7 +65,7 @@ typedef struct
 
 /****************************cmd_id命令码说明****************************/
 /****************************cmd_id命令码说明****************************/
-//  V1.6.1 裁判系统串口协议
+
 /* 命令码ID,用来判断接收的是什么数据 */
 typedef enum
 {
@@ -103,7 +103,7 @@ typedef enum
 	LEN_robot_hurt = 1,							 // 0x0206
 	LEN_shoot_data = 7,							 // 0x0207
 	LEN_receive_data = 6 + Communicate_Data_LEN, // 0x0301
-    LEN__custom_robot = 30,                      // 0x0302
+    LEN_self_control = 15,                     // 0x0303
     LEN_remote_control = 12,                     // 0x0304
 
 } JudgeDataLength_e;
@@ -128,23 +128,23 @@ typedef struct
 /* ID: 0x0003  Byte:  32    比赛机器人血量数据 */
 typedef struct
 {
-    uint16_t red_1_robot_HP;
-    uint16_t red_2_robot_HP;
-    uint16_t red_3_robot_HP;
-    uint16_t red_4_robot_HP;
-    uint16_t red_5_robot_HP;
-    uint16_t red_7_robot_HP;
-    uint16_t red_outpost_HP;
-    uint16_t red_base_HP;
-    uint16_t blue_1_robot_HP;
-    uint16_t blue_2_robot_HP;
-    uint16_t blue_3_robot_HP;
-    uint16_t blue_4_robot_HP;
-    uint16_t blue_5_robot_HP;
-    uint16_t blue_7_robot_HP;
-    uint16_t blue_outpost_HP;
-    uint16_t blue_base_HP;
-}ext_game_robot_HP_t;
+	uint16_t red_1_robot_HP;
+	uint16_t red_2_robot_HP;
+	uint16_t red_3_robot_HP;
+	uint16_t red_4_robot_HP;
+	uint16_t red_5_robot_HP;
+	uint16_t red_7_robot_HP;
+	uint16_t red_outpost_HP;
+	uint16_t red_base_HP;
+	uint16_t blue_1_robot_HP;
+	uint16_t blue_2_robot_HP;
+	uint16_t blue_3_robot_HP;
+	uint16_t blue_4_robot_HP;
+	uint16_t blue_5_robot_HP;
+	uint16_t blue_7_robot_HP;
+	uint16_t blue_outpost_HP;
+	uint16_t blue_base_HP;
+} ext_game_robot_HP_t;
 
 /* ID: 0x0101  Byte:  4    场地事件数据 */
 typedef struct
@@ -152,7 +152,7 @@ typedef struct
 	uint32_t event_type;
 } ext_event_data_t;
 
-/* ID: 0x0102  Byte:  4    场地补给站动作标识数据 */
+/* ID: 0x0102  Byte:  3    场地补给站动作标识数据 */
 typedef struct
 {
 	uint8_t supply_projectile_id;
@@ -161,29 +161,29 @@ typedef struct
 	uint8_t supply_projectile_num;
 } ext_supply_projectile_action_t;
 
-/* ID: 0X0201  Byte: 13    机器人性能体系数据 */
+/* ID: 0X0201  Byte: 27    V1.6.1 机器人状态数据 */
 typedef struct
 {
-    uint8_t robot_id;
-    uint8_t robot_level;
-    uint16_t current_HP;
-    uint16_t maximum_HP;
-    uint16_t shooter_barrel_cooling_value;
+	uint8_t robot_id;
+	uint8_t robot_level;
+	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;
+	uint16_t chassis_power_limit;
 
-    uint8_t power_management_gimbal_output : 1;
-    uint8_t power_management_chassis_output : 1;
-    uint8_t power_management_shooter_output : 1;
+	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: 16    实时功率热量数据 */
+/* ID: 0X0202  Byte: 14    V1.6.1实时功率热量数据 */
 typedef struct
 {
-    uint16_t chassis_voltage;
-    uint16_t chassis_current;
-    float chassis_power;
-    uint16_t buffer_energy;
+	uint16_t chassis_voltage;
+	uint16_t chassis_current;
+	float chassis_power;		   // 瞬时功率
+	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;
@@ -198,22 +198,16 @@ typedef struct
 	float yaw;
 } ext_game_robot_pos_t;
 
-/* ID: 0x0204  Byte:  6    机器人增益数据 */
+/* ID: 0x0204  Byte:  1    机器人增益数据 */
 typedef struct
 {
-    uint8_t recovery_buff;
-    uint8_t cooling_buff;
-    uint8_t defence_buff;
-    uint8_t vulnerability_buff;
-    uint16_t attack_buff;
-    uint8_t power_rune_buff;
+	uint8_t power_rune_buff;
 } ext_buff_musk_t;
 
-/* ID: 0x0205  Byte: 2    空中机器人能量状态数据 */
+/* ID: 0x0205  Byte:  1    空中机器人能量状态数据 */
 typedef struct
 {
-    uint8_t airforce_status;
-    uint8_t time_remain;
+	uint8_t attack_time;
 } aerial_robot_energy_t;
 
 /* ID: 0x0206  Byte:  1    伤害状态数据 */
@@ -233,7 +227,7 @@ typedef struct
 } ext_shoot_data_t;
 
 /****************************图传链路数据****************************/
-/* ID: 0x0304  Byte:  12    图传链路键鼠遥控数据 */
+/* ID: 0x0304  Byte:  7    图传链路键鼠遥控数据 */
 typedef struct
 {
     int16_t mouse_x;

modules/referee/referee_task.c

@@ -14,15 +14,10 @@
 #include "referee_UI.h"
 #include "string.h"
 #include "cmsis_os.h"
-#include "chassis.h"
-#include "super_cap.h"
-
-extern SuperCapInstance *cap;                        // 超级电容
+Referee_Interactive_info_t ui_data;
 static Referee_Interactive_info_t *Interactive_data; // UI绘制需要的机器人状态数据
 static referee_info_t *referee_recv_info;            // 接收到的裁判系统数据
-Referee_Interactive_info_t ui_data;
 uint8_t UI_Seq;                                      // 包序号，供整个referee文件使用
-
 // @todo 不应该使用全局变量
 
 /**
@@ -54,101 +49,141 @@ 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_yuan[2];  // 射击圆心
-static Graph_Data_t UI_Energy[6];      // 电容能量条
-static String_Data_t UI_State_sta[7];  // 机器人状态,静态只需画一次
-static String_Data_t UI_State_dyn[7];  // 机器人状态,动态先add才能change
-static uint32_t shoot_line_location[10] = {540, 960, 490, 515, 565};
-static String_Data_t UI_prompt_sta[3];  // 操作手提示
+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,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_Orange, 2, 960, 200, 960, 600);
-    UICircleDraw(&UI_shoot_yuan[0],"sy0",UI_Graph_ADD,7,UI_Color_Yellow,2,960,540,80);
-    UIGraphRefresh(&referee_recv_info->referee_id, 5, UI_shoot_line[0], UI_shoot_line[1], UI_shoot_line[2], UI_shoot_line[3],UI_shoot_yuan[0]);
-    UILineDraw(&UI_shoot_line[4], "sl4", UI_Graph_ADD,7, UI_Color_Green, 2, 1020, 450, 1020, 600);
-    UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_shoot_line[4]);
 
-    // 绘制车辆状态标志指示，静态
-    UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Main, 20, 2, 10, 750, "E.spin:");
+    // 绘制发射基准线
+    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:");
     UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[0]);
-//    UICharDraw(&UI_State_sta[1], "ss1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 150, 700, "gimbal:");
-//    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[1]);
-//    UICharDraw(&UI_State_sta[2], "ss2", UI_Graph_ADD, 8, UI_Color_Orange, 15, 2, 150, 650, "shoot:");
-//    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[2]);
-    UICharDraw(&UI_State_sta[3], "ss3", UI_Graph_ADD, 8, UI_Color_Pink, 20, 2, 10, 700, "F.frict:");
+    UICharDraw(&UI_State_sta[1], "ss1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 150, 700, "gimbal:");
+    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[1]);
+    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_Main, 20, 2, 10, 650, "B.lid:");
-    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[4]);
-    UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 8, UI_Color_Pink, 20, 2, 10, 600, "Q.heat:");
-    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[5]);
-    UICharDraw(&UI_State_sta[6], "ss6", UI_Graph_ADD, 8, UI_Color_Pink, 20, 2, 10, 550, "C.ce:");
-    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[6]);
-//    UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 150, 800, "load:");
-//    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[5]);
 
-    // 操作手提示
-    UICharDraw(&UI_prompt_sta[0], "ss8", UI_Graph_ADD,8, UI_Color_Pink, 30, 4, 1650, 700, "bie huang");
-    UICharRefresh(&referee_recv_info->referee_id, UI_prompt_sta[0]);
-
-    // 底盘功率显示，静态
-//    UICharDraw(&UI_State_sta[6], "ss6", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 620, 230, "MAX:");
-//    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[6]);
 
     // 绘制车辆状态标志，动态
     // 由于初始化时xxx_last_mode默认为0，所以此处对应UI也应该设为0时对应的UI，防止模式不变的情况下无法置位flag，导致UI无法刷新
-    UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_ADD, 8, UI_Color_Main, 20, 2, 170, 750, "off");
+    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");
-//    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, 20, 2, 170, 700, "off");
+    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, 20, 2, 170, 650, "off");
-    UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]);
-    UICharDraw(&UI_State_dyn[5], "sd5", UI_Graph_ADD, 8, UI_Color_Pink, 20, 2, 170, 600, "on ");
-    UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[5]);
-    UICharDraw(&UI_State_dyn[6], "sd8", UI_Graph_ADD, 8, UI_Color_Pink, 20, 2, 170, 550, "off");
-    UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[6]);
-//    UICharDraw(&UI_State_dyn[5], "sd5", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 270, 800, "999");
-//    UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[5]);
 
+//    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, 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]);
+    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], "sd6", UI_Graph_ADD, 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]);
+    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);
 
-    // 电容电压
-    UIFloatDraw(&UI_Energy[2], "sd7", UI_Graph_ADD, 8, UI_Color_Pink, 18, 2,2, 850, 280,Interactive_data->Cap_Data.cap_vol);
-    UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[2]);
-    // 电容能量条
-//    UILineDraw(&UI_Energy[3], "sd8", UI_Graph_ADD, 8, UI_Color_Pink, 30, 720, 160, (uint32_t)(720 + (Interactive_data->Cap_Data.cap_vol-1200) * 0.416), 160);
-//    UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[3]);
+    UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]);
+}
 
-    // 发弹量条框
-    UILineDraw(&UI_Energy[4], "ss7", UI_Graph_ADD, 7, UI_Color_Pink, 2, 1325, 500, 1480, 500);
-    UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[4]);
+// 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
+static uint8_t count = 0;
+static uint16_t count1 = 0;
+static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) // 测试用函数，实现模式自动变化
+{
+    count++;
+    if (count >= 50)
+    {
+        count = 0;
+        count1++;
+    }
+    switch (count1 % 4)
+    {
+    case 0:
+    {
+        _Interactive_data->chassis_mode = CHASSIS_ZERO_FORCE;
+        _Interactive_data->gimbal_mode = GIMBAL_ZERO_FORCE;
+        _Interactive_data->shoot_mode = SHOOT_ON;
+        _Interactive_data->friction_mode = FRICTION_ON;
+        _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;
+        break;
+    }
+    case 1:
+    {
+        _Interactive_data->chassis_mode = CHASSIS_ROTATE;
+        _Interactive_data->gimbal_mode = GIMBAL_FREE_MODE;
+        _Interactive_data->shoot_mode = SHOOT_OFF;
+        _Interactive_data->friction_mode = FRICTION_OFF;
+        _Interactive_data->tele_mode = TELE_CLOSE;
+        break;
+    }
+    case 2:
+    {
+        _Interactive_data->chassis_mode = CHASSIS_NO_FOLLOW;
+        _Interactive_data->gimbal_mode = GIMBAL_GYRO_MODE;
+        _Interactive_data->shoot_mode = SHOOT_ON;
+        _Interactive_data->friction_mode = FRICTION_ON;
+        _Interactive_data->tele_mode = TELE_OPEN;
+        break;
+    }
+    case 3:
+    {
+        _Interactive_data->chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
+        _Interactive_data->gimbal_mode = GIMBAL_ZERO_FORCE;
+        _Interactive_data->shoot_mode = SHOOT_OFF;
+        _Interactive_data->friction_mode = FRICTION_OFF;
+        _Interactive_data->tele_mode = TELE_CLOSE;
+        break;
+    }
+    default:
+        break;
+    }
 }
 
 static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data)
@@ -158,125 +193,116 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i
     if (_Interactive_data->Referee_Interactive_Flag.chassis_flag == 1)
     {
         switch (_Interactive_data->chassis_mode)
-            // 此处注意字数对齐问题，字数相同才能覆盖掉
         {
-            case CHASSIS_ZERO_FORCE:
-                 UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 20, 2, 170, 750, "off");
-                 UICharDraw(&UI_State_dyn[6], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 20, 2, 170, 550,  "off");
-                 break;
-            case CHASSIS_ROTATE:
-                 UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 20, 2, 170, 750, "on ");
-                 UICharDraw(&UI_State_dyn[6], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 20, 2, 170, 550,  "off");
-                 break;
-            case CHASSIS_SIDEWAYS:
-                 UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 20, 2, 170, 750, "off");
-                 UICharDraw(&UI_State_dyn[6], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 20, 2, 170, 550,  "on ");
-                break;
-            case CHASSIS_NO_FOLLOW:
-                 UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 20, 2, 170, 750, "off");
-                 UICharDraw(&UI_State_dyn[6], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 20, 2, 170, 550,  "off");
-                break;
-            case CHASSIS_FOLLOW_GIMBAL_YAW:
-                 UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 20, 2, 170, 750, "off");
-                 UICharDraw(&UI_State_dyn[6], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 20, 2, 170, 550,  "off");
-                break;
+        case CHASSIS_ZERO_FORCE:
+            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce");
+            break;
+        case CHASSIS_ROTATE:
+            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "rotate   ");
+            // 此处注意字数对齐问题，字数相同才能覆盖掉
+            break;
+        case CHASSIS_NO_FOLLOW:
+            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "nofollow ");
+            break;
+        case CHASSIS_FOLLOW_GIMBAL_YAW:
+            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "follow   ");
+            break;
         }
-        UICharRefresh(&referee_recv_info->referee_id,UI_State_dyn[0]);
-        UICharRefresh(&referee_recv_info->referee_id,UI_State_dyn[6]);
+        UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]);
         _Interactive_data->Referee_Interactive_Flag.chassis_flag = 0;
     }
     // gimbal
-//    if (_Interactive_data->Referee_Interactive_Flag.gimbal_flag == 1)
-//    {
-//        switch (_Interactive_data->gimbal_mode)
-//        {
-//        case GIMBAL_ZERO_FORCE:
-//        {
-//            UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce");
-//            break;
-//        }
-//        case GIMBAL_FREE_MODE:
-//        {
-//            UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "free     ");
-//            break;
-//        }
-//        case GIMBAL_GYRO_MODE:
-//        {
-//            UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "gyro     ");
-//            break;
-//        }
-//        }
-//        UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
-//        _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 0;
-//    }
+    if (_Interactive_data->Referee_Interactive_Flag.gimbal_flag == 1)
+    {
+        switch (_Interactive_data->gimbal_mode)
+        {
+        case GIMBAL_ZERO_FORCE:
+        {
+            UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce");
+            break;
+        }
+        case GIMBAL_FREE_MODE:
+        {
+            UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "free     ");
+            break;
+        }
+        case GIMBAL_GYRO_MODE:
+        {
+            UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "gyro     ");
+            break;
+        }
+        }
+        UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
+        _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 0;
+    }
     // shoot
-//    if (_Interactive_data->Referee_Interactive_Flag.shoot_flag == 1)
-//    {
-//        UICharDraw(&UI_State_dyn[2], "sd2", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 650, _Interactive_data->shoot_mode == SHOOT_ON ? "on " : "off");
-//        UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[2]);
-//        _Interactive_data->Referee_Interactive_Flag.shoot_flag = 0;
-//    }
+    if (_Interactive_data->Referee_Interactive_Flag.shoot_flag == 1)
+    {
+        UICharDraw(&UI_State_dyn[2], "sd2", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 650, _Interactive_data->shoot_mode == SHOOT_ON ? "on " : "off");
+        UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[2]);
+        _Interactive_data->Referee_Interactive_Flag.shoot_flag = 0;
+    }
     // friction
     if (_Interactive_data->Referee_Interactive_Flag.friction_flag == 1)
     {
-        UICharDraw(&UI_State_dyn[3], "sd3", UI_Graph_Change, 8, UI_Color_Pink, 20, 2, 170, 700, _Interactive_data->friction_mode == FRICTION_ON ? "on " : "off");
+        UICharDraw(&UI_State_dyn[3], "sd3", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 600, _Interactive_data->friction_mode == FRICTION_ON ? "on " : "off");
         UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[3]);
         _Interactive_data->Referee_Interactive_Flag.friction_flag = 0;
     }
     // lid
-    if (_Interactive_data->Referee_Interactive_Flag.lid_flag == 1)
-    {
-        UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_Change, 8, UI_Color_Pink, 20, 2, 170, 650, _Interactive_data->lid_mode == LID_OPEN ? "on " : "off");
-        UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]);
-        _Interactive_data->Referee_Interactive_Flag.lid_flag = 0;
-    }
-    // heat
-    if (_Interactive_data->Referee_Interactive_Flag.heat_flag == 1)
-    {
-        UICharDraw(&UI_State_dyn[5], "sd5", UI_Graph_Change, 8, UI_Color_Pink, 20, 2, 170, 600, _Interactive_data->heat_mode == HEAT_OPEN ? "on " : "off");
-        UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[5]);
-        _Interactive_data->Referee_Interactive_Flag.heat_flag = 0;
-    }
-    // loader_mode
-//    if (_Interactive_data->Referee_Interactive_Flag.load_flag == 1)
+//    if (_Interactive_data->Referee_Interactive_Flag.lid_flag == 1)
 //    {
-//        switch (loader_flag)
-//        {
-//            case 2:
-//            {
-//                UICharDraw(&UI_State_dyn[5], "sd5", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 800, " 1 ");
-//                break;
-//            }
-//            case 1:
-//            {
-//                UICharDraw(&UI_State_dyn[5], "sd5", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 800, " 3 ");
-//                break;
-//            }
-//            case 0:
-//            {
-//                UICharDraw(&UI_State_dyn[5], "sd5", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 800, "999");
-//                break;
-//            }
-//        }
-//        UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[5]);
-//        _Interactive_data->Referee_Interactive_Flag.load_flag = 0;
+//        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], "sd6", 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, 2, UI_Energy[1]);
+        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;
     }
-    UIFloatDraw(&UI_Energy[2], "sd7", UI_Graph_Change, 8, UI_Color_Pink, 18, 2,2, 850, 280,Interactive_data->Cap_Data.cap_vol);
-    UIGraphRefresh(&referee_recv_info->referee_id, 1,UI_Energy[2]);
-//    UILineDraw(&UI_Energy[3], "sd8", UI_Graph_Change, 8, UI_Color_Pink, 30, 720, 160, (uint32_t)(720 + (Interactive_data->Cap_Data.cap_vol-1200) * 0.416), 160);
-//    UIGraphRefresh(&referee_recv_info->referee_id, 1, &UI_Energy[3]);
-    //绘制开火建议
-    if (_Interactive_data->Referee_Interactive_Flag.aim_flag == 1) {
-        UICircleDraw(&UI_shoot_yuan[0], "sy0", UI_Graph_Change, 7, _Interactive_data->aim_fire == 0 ? UI_Color_Yellow : UI_Color_Main, 2, 960, 540, 80);
-        UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_shoot_yuan[0]);
-        _Interactive_data->Referee_Interactive_Flag.aim_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;
     }
 }
 
@@ -294,17 +320,17 @@ static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data)
         _Interactive_data->chassis_last_mode = _Interactive_data->chassis_mode;
     }
 
-//    if (_Interactive_data->gimbal_mode != _Interactive_data->gimbal_last_mode)
-//    {
-//        _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 1;
-//        _Interactive_data->gimbal_last_mode = _Interactive_data->gimbal_mode;
-//    }
+    if (_Interactive_data->gimbal_mode != _Interactive_data->gimbal_last_mode)
+    {
+        _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 1;
+        _Interactive_data->gimbal_last_mode = _Interactive_data->gimbal_mode;
+    }
 
-//    if (_Interactive_data->shoot_mode != _Interactive_data->shoot_last_mode)
-//    {
-//        _Interactive_data->Referee_Interactive_Flag.shoot_flag = 1;
-//        _Interactive_data->shoot_last_mode = _Interactive_data->shoot_mode;
-//    }
+    if (_Interactive_data->shoot_mode != _Interactive_data->shoot_last_mode)
+    {
+        _Interactive_data->Referee_Interactive_Flag.shoot_flag = 1;
+        _Interactive_data->shoot_last_mode = _Interactive_data->shoot_mode;
+    }
 
     if (_Interactive_data->friction_mode != _Interactive_data->friction_last_mode)
     {
@@ -312,16 +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;
-    }
-
-    if (_Interactive_data->heat_mode != _Interactive_data->heat_last_mode)
-    {
-        _Interactive_data->Referee_Interactive_Flag.heat_flag = 1;
-        _Interactive_data->heat_last_mode = _Interactive_data->heat_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)
@@ -329,22 +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->Cap_Data.cap_vol != _Interactive_data->Cap_last_Data.cap_vol)
-//    {
-//        _Interactive_data->Referee_Interactive_Flag.cap_flag = 1;
-//        _Interactive_data->Cap_last_Data.cap_vol = _Interactive_data->Cap_Data.cap_vol;
-//    }
-
-    if (_Interactive_data->aim_fire != _Interactive_data->aim_last_fire)
+    if (_Interactive_data->Chassis_Power_Data.cap_vol != _Interactive_data->Chassis_last_Power_Data.cap_vol)
     {
-        _Interactive_data->Referee_Interactive_Flag.aim_flag = 1;
-        _Interactive_data->aim_last_fire = _Interactive_data->aim_fire;
+        _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;
     }
-
-//    if (_Interactive_data->loader_mode != _Interactive_data->loader_last_mode)
-//    {
-//        _Interactive_data->Referee_Interactive_Flag.load_flag = 1;
-//        _Interactive_data->loader_last_mode = _Interactive_data->loader_mode;
-//    }
 }

modules/referee/rm_referee.h

@@ -7,8 +7,6 @@
 #include "bsp_usart.h"
 #include "FreeRTOS.h"
 
-
-
 extern uint8_t UI_Seq;
 
 #pragma pack(1)
@@ -40,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;
@@ -56,10 +55,10 @@ typedef struct
 	uint32_t lid_flag : 1;
 	uint32_t friction_flag : 1;
 	uint32_t Power_flag : 1;
-	uint32_t aim_flag : 1;
-	uint32_t cap_flag : 1;
-	uint32_t load_flag : 1;
-	uint32_t heat_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绘制与机器人车间通信的需要的其他非裁判系统数据
@@ -69,26 +68,23 @@ typedef struct
 	// 为UI绘制以及交互数据所用
 	chassis_mode_e chassis_mode;			 // 底盘模式
 	gimbal_mode_e gimbal_mode;				 // 云台模式
-	shoot_mode_e shoot_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 aim_fire;                        // 开火建议
-    loader_mode_e loader_mode;               // 发射模式
-    heat_mode_e heat_mode;                   // 热量控制
-    Cap_Data_s Cap_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 aim_last_fire;
-    loader_mode_e loader_last_mode;
-    heat_mode_e heat_last_mode;
-    Cap_Data_s Cap_last_Data;
+    uint8_t last_Vision_fire;
+    uint8_t Last_Motor_Temperature;
 
 } Referee_Interactive_info_t;
 

modules/referee/vision_transfer.c

@@ -1,162 +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__custom_robot);
-                        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);
-}
+/**
+ * @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);
+}

modules/referee/vision_transfer.h

@@ -1,46 +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
+#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

modules/remote/remote_control.c

@@ -5,20 +5,22 @@
 #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;
 
-// 图传拥有的串口实例
-static USARTInstance *vt_usart_instance;
-static DaemonInstance *vt_daemon_instance;
+static DaemonInstance *rc_daemon_instance;
 
 /**
  * @brief 矫正遥控器摇杆的值,超过660或者小于-660的值都认为是无效值,置0
@@ -101,18 +103,13 @@ static void RemoteControlRxCallback()
     sbus_to_rc(rc_usart_instance->recv_buff); // 进行协议解析
 }
 
-//static void RemoteControlvtCallback()
-//{
-//    DaemonReload(vt_daemon_instance);         // 先喂狗
-//    sbus_to_rc(vt_usart_instance->recv_buff); // 进行协议解析
-//}
-
 /**
  * @brief 遥控器离线的回调函数,注册到守护进程中,串口掉线时调用
  *
  */
 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");
@@ -143,4 +140,4 @@ uint8_t RemoteControlIsOnline()
     if (rc_init_flag)
         return DaemonIsOnline(rc_daemon_instance);
     return 0;
-}
+}

modules/remote/remote_control.h

@@ -137,7 +137,6 @@ typedef struct
  */
 RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle);
 
-//VT_ctrl_t *RemoteControlInit(UART_HandleTypeDef *vt_usart_handle);
 
 /**
  * @brief 检查遥控器是否在线,若尚未初始化也视为离线

modules/robotics/matrix.cpp

@@ -1,24 +0,0 @@
-/**
- ******************************************************************************
- * @file    matrix.cpp/h
- * @brief   Matrix/vector calculation. 矩阵/向量运算
- * @author  Spoon Guan
- ******************************************************************************
- * Copyright (c) 2023 Team JiaoLong-SJTU
- * All rights reserved.
- ******************************************************************************
- */
-
-#include "matrix.h"
-
-// hat of vector
-Matrixf<3, 3> vector3f::hat(Matrixf<3, 1> vec) {
-  float hat[9] = {0,          -vec[2][0], vec[1][0], vec[2][0], 0,
-                  -vec[0][0], -vec[1][0], vec[0][0], 0};
-  return Matrixf<3, 3>(hat);
-}
-
-// cross product
-Matrixf<3, 1> vector3f::cross(Matrixf<3, 1> vec1, Matrixf<3, 1> vec2) {
-  return vector3f::hat(vec1) * vec2;
-}

modules/robotics/matrix.h

@@ -1,259 +0,0 @@
-/**
- ******************************************************************************
- * @file    matrix.cpp/h
- * @brief   Matrix/vector calculation. 矩阵/向量运算
- * @author  Spoon Guan
- ******************************************************************************
- * Copyright (c) 2023 Team JiaoLong-SJTU
- * All rights reserved.
- ******************************************************************************
- */
-
-#ifndef MATRIX_H
-#define MATRIX_H
-
-//#include "arm_math.h"
-#include "user_lib.h"
-// Matrix class
-template <int _rows, int _cols>
-class Matrixf {
- public:
-  // Constructor without input data
-  Matrixf(void) : rows_(_rows), cols_(_cols) {
-    arm_mat_init_f32(&arm_mat_, _rows, _cols, this->data_);
-  }
-  // Constructor with input data
-  Matrixf(float data[_rows * _cols]) : Matrixf() {
-    memcpy(this->data_, data, _rows * _cols * sizeof(float));
-  }
-  // Copy constructor
-  Matrixf(const Matrixf<_rows, _cols>& mat) : Matrixf() {
-    memcpy(this->data_, mat.data_, _rows * _cols * sizeof(float));
-  }
-  // Destructor
-  ~Matrixf(void) {}
-
-  // Row size
-  int rows(void) { return _rows; }
-  // Column size
-  int cols(void) { return _cols; }
-
-  // Element
-  float* operator[](const int& row) { return &this->data_[row * _cols]; }
-
-  // Operators
-  Matrixf<_rows, _cols>& operator=(const Matrixf<_rows, _cols> mat) {
-    memcpy(this->data_, mat.data_, _rows * _cols * sizeof(float));
-    return *this;
-  }
-  Matrixf<_rows, _cols>& operator+=(const Matrixf<_rows, _cols> mat) {
-    arm_status s;
-    s = arm_mat_add_f32(&this->arm_mat_, &mat.arm_mat_, &this->arm_mat_);
-    return *this;
-  }
-  Matrixf<_rows, _cols>& operator-=(const Matrixf<_rows, _cols> mat) {
-    arm_status s;
-    s = arm_mat_sub_f32(&this->arm_mat_, &mat.arm_mat_, &this->arm_mat_);
-    return *this;
-  }
-  Matrixf<_rows, _cols>& operator*=(const float& val) {
-    arm_status s;
-    s = arm_mat_scale_f32(&this->arm_mat_, val, &this->arm_mat_);
-    return *this;
-  }
-  Matrixf<_rows, _cols>& operator/=(const float& val) {
-    arm_status s;
-    s = arm_mat_scale_f32(&this->arm_mat_, 1.f / val, &this->arm_mat_);
-    return *this;
-  }
-  Matrixf<_rows, _cols> operator+(const Matrixf<_rows, _cols>& mat) {
-    arm_status s;
-    Matrixf<_rows, _cols> res;
-    s = arm_mat_add_f32(&this->arm_mat_, &mat.arm_mat_, &res.arm_mat_);
-    return res;
-  }
-  Matrixf<_rows, _cols> operator-(const Matrixf<_rows, _cols>& mat) {
-    arm_status s;
-    Matrixf<_rows, _cols> res;
-    s = arm_mat_sub_f32(&this->arm_mat_, &mat.arm_mat_, &res.arm_mat_);
-    return res;
-  }
-  Matrixf<_rows, _cols> operator*(const float& val) {
-    arm_status s;
-    Matrixf<_rows, _cols> res;
-    s = arm_mat_scale_f32(&this->arm_mat_, val, &res.arm_mat_);
-    return res;
-  }
-  friend Matrixf<_rows, _cols> operator*(const float& val,
-                                         const Matrixf<_rows, _cols>& mat) {
-    arm_status s;
-    Matrixf<_rows, _cols> res;
-    s = arm_mat_scale_f32(&mat.arm_mat_, val, &res.arm_mat_);
-    return res;
-  }
-  Matrixf<_rows, _cols> operator/(const float& val) {
-    arm_status s;
-    Matrixf<_rows, _cols> res;
-    s = arm_mat_scale_f32(&this->arm_mat_, 1.f / val, &res.arm_mat_);
-    return res;
-  }
-  // Matrix multiplication
-  template <int cols>
-  friend Matrixf<_rows, cols> operator*(const Matrixf<_rows, _cols>& mat1,
-                                        const Matrixf<_cols, cols>& mat2) {
-    arm_status s;
-    Matrixf<_rows, cols> res;
-    s = arm_mat_mult_f32(&mat1.arm_mat_, &mat2.arm_mat_, &res.arm_mat_);
-    return res;
-  }
-
-  // Submatrix
-  template <int rows, int cols>
-  Matrixf<rows, cols> block(const int& start_row, const int& start_col) {
-    Matrixf<rows, cols> res;
-    for (int row = start_row; row < start_row + rows; row++) {
-      memcpy((float*)res[0] + (row - start_row) * cols,
-             (float*)this->data_ + row * _cols + start_col,
-             cols * sizeof(float));
-    }
-    return res;
-  }
-  // Specific row
-  Matrixf<1, _cols> row(const int& row) { return block<1, _cols>(row, 0); }
-  // Specific column
-  Matrixf<_rows, 1> col(const int& col) { return block<_rows, 1>(0, col); }
-
-  // Transpose
-  Matrixf<_cols, _rows> trans(void) {
-    Matrixf<_cols, _rows> res;
-    arm_mat_trans_f32(&arm_mat_, &res.arm_mat_);
-    return res;
-  }
-  // Trace
-  float trace(void) {
-    float res = 0;
-    for (int i = 0; i < fmin(_rows, _cols); i++) {
-      res += (*this)[i][i];
-    }
-    return res;
-  }
-  // Norm
-  float norm(void) { return sqrtf((this->trans() * *this)[0][0]); }
-
- public:
-  // arm matrix instance
-  arm_matrix_instance_f32 arm_mat_;
-
- protected:
-  // size
-  int rows_, cols_;
-  // data
-  float data_[_rows * _cols];
-};
-
-// Matrix funtions
-namespace matrixf {
-
-// Special Matrices
-// Zero matrix
-template <int _rows, int _cols>
-Matrixf<_rows, _cols> zeros(void) {
-  float data[_rows * _cols] = {0};
-  return Matrixf<_rows, _cols>(data);
-}
-// Ones matrix
-template <int _rows, int _cols>
-Matrixf<_rows, _cols> ones(void) {
-  float data[_rows * _cols] = {0};
-  for (int i = 0; i < _rows * _cols; i++) {
-    data[i] = 1;
-  }
-  return Matrixf<_rows, _cols>(data);
-}
-// Identity matrix
-template <int _rows, int _cols>
-Matrixf<_rows, _cols> eye(void) {
-  float data[_rows * _cols] = {0};
-  for (int i = 0; i < fmin(_rows, _cols); i++) {
-    data[i * _cols + i] = 1;
-  }
-  return Matrixf<_rows, _cols>(data);
-}
-// Diagonal matrix
-template <int _rows, int _cols>
-Matrixf<_rows, _cols> diag(Matrixf<_rows, 1> vec) {
-  Matrixf<_rows, _cols> res = matrixf::zeros<_rows, _cols>();
-  for (int i = 0; i < fmin(_rows, _cols); i++) {
-    res[i][i] = vec[i][0];
-  }
-  return res;
-}
-
-// Inverse
-template <int _dim>
-Matrixf<_dim, _dim> inv(Matrixf<_dim, _dim> mat) {
-  arm_status s;
-  // extended matrix [A|I]
-  Matrixf<_dim, 2 * _dim> ext_mat = matrixf::zeros<_dim, 2 * _dim>();
-  for (int i = 0; i < _dim; i++) {
-    memcpy(ext_mat[i], mat[i], _dim * sizeof(float));
-    ext_mat[i][_dim + i] = 1;
-  }
-  // elimination
-  for (int i = 0; i < _dim; i++) {
-    // find maximum absolute value in the first column in lower right block
-    float abs_max = fabs(ext_mat[i][i]);
-    int abs_max_row = i;
-    for (int row = i; row < _dim; row++) {
-      if (abs_max < fabs(ext_mat[row][i])) {
-        abs_max = fabs(ext_mat[row][i]);
-        abs_max_row = row;
-      }
-    }
-    if (abs_max < 1e-12f) {  // singular
-      return matrixf::zeros<_dim, _dim>();
-      s = ARM_MATH_SINGULAR;
-    }
-    if (abs_max_row != i) {  // row exchange
-      float tmp;
-      Matrixf<1, 2 * _dim> row_i = ext_mat.row(i);
-      Matrixf<1, 2 * _dim> row_abs_max = ext_mat.row(abs_max_row);
-      memcpy(ext_mat[i], row_abs_max[0], 2 * _dim * sizeof(float));
-      memcpy(ext_mat[abs_max_row], row_i[0], 2 * _dim * sizeof(float));
-    }
-    float k = 1.f / ext_mat[i][i];
-    for (int col = i; col < 2 * _dim; col++) {
-      ext_mat[i][col] *= k;
-    }
-    for (int row = 0; row < _dim; row++) {
-      if (row == i) {
-        continue;
-      }
-      k = ext_mat[row][i];
-      for (int j = i; j < 2 * _dim; j++) {
-        ext_mat[row][j] -= k * ext_mat[i][j];
-      }
-    }
-  }
-  // inv = ext_mat(:,n+1:2n)
-  s = ARM_MATH_SUCCESS;
-  Matrixf<_dim, _dim> res;
-  for (int i = 0; i < _dim; i++) {
-    memcpy(res[i], &ext_mat[i][_dim], _dim * sizeof(float));
-  }
-  return res;
-}
-
-}  // namespace matrixf
-
-namespace vector3f {
-
-// hat of vector
-Matrixf<3, 3> hat(Matrixf<3, 1> vec);
-
-// cross product
-Matrixf<3, 1> cross(Matrixf<3, 1> vec1, Matrixf<3, 1> vec2);
-
-}  // namespace vector3f
-
-#endif  // MATRIX_H

modules/robotics/robotics.cpp

@@ -1,340 +0,0 @@
-/**
- ******************************************************************************
- * @file    robotics.cpp/h
- * @brief   Robotic toolbox on STM32. STM32机器人学库
- * @author  Spoon Guan
- * @ref     [1] SJTU ME385-2, Robotics, Y.Ding
- *          [2] Bruno Siciliano, et al., Robotics: Modelling, Planning and
- *              Control, Springer, 2010.
- *          [3] R.Murry, Z.X.Li, and S.Sastry, A Mathematical Introduction
- *              to Robotic Manipulation, CRC Press, 1994.
- ******************************************************************************
- * Copyright (c) 2023 Team JiaoLong-SJTU
- * All rights reserved.
- ******************************************************************************
- */
-
-#include "robotics.h"
-
-Matrixf<3, 1> robotics::r2rpy(Matrixf<3, 3> R) {
-  float rpy[3] = {
-      atan2f(R[1][0], R[0][0]),                                        // yaw
-      atan2f(-R[2][0], sqrtf(R[2][1] * R[2][1] + R[2][2] * R[2][2])),  // pitch
-      atan2f(R[2][1], R[2][2])                                         // roll
-  };
-  return Matrixf<3, 1>(rpy);
-}
-
-Matrixf<3, 3> robotics::rpy2r(Matrixf<3, 1> rpy) {
-  float c[3] = {cosf(rpy[0][0]), cosf(rpy[1][0]), cosf(rpy[2][0])};
-  float s[3] = {sinf(rpy[0][0]), sinf(rpy[1][0]), sinf(rpy[2][0])};
-  float R[9] = {
-      c[0] * c[1],                       // R11
-      c[0] * s[1] * s[2] - s[0] * c[2],  // R12
-      c[0] * s[1] * c[2] + s[0] * s[2],  // R13
-      s[0] * c[1],                       // R21
-      s[0] * s[1] * s[2] + c[0] * c[2],  // R22
-      s[0] * s[1] * c[2] - c[0] * s[2],  // R23
-      -s[1],                             // R31
-      c[1] * s[2],                       // R32
-      c[1] * c[2]                        // R33
-  };
-  return Matrixf<3, 3>(R);
-}
-
-Matrixf<4, 1> robotics::r2angvec(Matrixf<3, 3> R) {
-  float theta = acosf(math::limit(0.5f * (R.trace() - 1), -1, 1));
-  if (theta == 0 || theta == PI) {
-    float angvec[4] = {
-        (1 + R[0][0] - R[1][1] - R[2][2]) / 4.0f,  // rx=(1+R11-R22-R33)/4
-        (1 - R[0][0] + R[1][1] - R[2][2]) / 4.0f,  // ry=(1-R11+R22-R33)/4
-        (1 - R[0][0] - R[1][1] + R[2][2]) / 4.0f,  // rz=(1-R11-R22+R33)/4
-        theta                                      // theta
-    };
-    return Matrixf<4, 1>(angvec);
-  }
-  float angvec[4] = {
-      (R[2][1] - R[1][2]) / (2.0f * sinf(theta)),  // rx=(R32-R23)/2sinθ
-      (R[0][2] - R[2][0]) / (2.0f * sinf(theta)),  // ry=(R13-R31)/2sinθ
-      (R[1][0] - R[0][1]) / (2.0f * sinf(theta)),  // rz=(R21-R12)/2sinθ
-      theta                                        // theta
-  };
-  return Matrixf<4, 1>(angvec);
-}
-
-Matrixf<3, 3> robotics::angvec2r(Matrixf<4, 1> angvec) {
-  float theta = angvec[3][0];
-  Matrixf<3, 1> r = angvec.block<3, 1>(0, 0);
-  Matrixf<3, 3> R;
-  // Rodrigues formula: R=I+ω^sinθ+ω^^2(1-cosθ)
-  R = matrixf::eye<3, 3>() + vector3f::hat(r) * sinf(theta) +
-      vector3f::hat(r) * vector3f::hat(r) * (1 - cosf(theta));
-  return R;
-}
-
-Matrixf<4, 1> robotics::r2quat(Matrixf<3, 3> R) {
-  float q[4] = {
-      0.5f * sqrtf(math::limit(R.trace(), -1, 1) + 1),  // q0=cos(θ/2)
-      math::sign(R[2][1] - R[1][2]) * 0.5f *
-          sqrtf(math::limit(R[0][0] - R[1][1] - R[2][2], -1, 1) +
-                1),  // q1=rx*sin(θ/2)
-      math::sign(R[0][2] - R[2][0]) * 0.5f *
-          sqrtf(math::limit(-R[0][0] + R[1][1] - R[2][2], -1, 1) +
-                1),  // q2=ry*sin(θ/2)
-      math::sign(R[1][0] - R[0][1]) * 0.5f *
-          sqrtf(math::limit(-R[0][0] - R[1][1] + R[2][2], -1, 1) +
-                1),  // q3=rz*sin(θ/2)
-  };
-  return (Matrixf<4, 1>(q) / Matrixf<4, 1>(q).norm());
-}
-
-Matrixf<3, 3> robotics::quat2r(Matrixf<4, 1> q) {
-  float R[9] = {
-      1 - 2.0f * (q[2][0] * q[2][0] + q[3][0] * q[3][0]),  // R11
-      2.0f * (q[1][0] * q[2][0] - q[0][0] * q[3][0]),      // R12
-      2.0f * (q[1][0] * q[3][0] + q[0][0] * q[2][0]),      // R13
-      2.0f * (q[1][0] * q[2][0] + q[0][0] * q[3][0]),      // R21
-      1 - 2.0f * (q[1][0] * q[1][0] + q[3][0] * q[3][0]),  // R22
-      2.0f * (q[2][0] * q[3][0] - q[0][0] * q[1][0]),      // R23
-      2.0f * (q[1][0] * q[3][0] - q[0][0] * q[2][0]),      // R31
-      2.0f * (q[2][0] * q[3][0] + q[0][0] * q[1][0]),      // R32
-      1 - 2.0f * (q[1][0] * q[1][0] + q[2][0] * q[2][0])   // R33
-  };
-  return Matrixf<3, 3>(R);
-}
-
-Matrixf<3, 1> robotics::quat2rpy(Matrixf<4, 1> q) {
-  float rpy[3] = {
-      atan2f(2.0f * (q[1][0] * q[2][0] + q[0][0] * q[3][0]),
-             1 - 2.0f * (q[2][0] * q[2][0] + q[3][0] * q[3][0])),  // yaw
-      asinf(2.0f * (q[0][0] * q[2][0] - q[1][0] * q[3][0])),       // pitch
-      atan2f(2.0f * (q[2][0] * q[3][0] + q[0][0] * q[1][0]),
-             1 - 2.0f * (q[1][0] * q[1][0] + q[2][0] * q[2][0]))  // rol
-  };
-  return Matrixf<3, 1>(rpy);
-}
-
-Matrixf<4, 1> robotics::rpy2quat(Matrixf<3, 1> rpy) {
-  float c[3] = {cosf(0.5f * rpy[0][0]), cosf(0.5f * rpy[1][0]),
-                cosf(0.5f * rpy[2][0])};  // cos(*/2)
-  float s[3] = {sinf(0.5f * rpy[0][0]), sinf(0.5f * rpy[1][0]),
-                sinf(0.5f * rpy[2][0])};  // sin(*/2)
-  float q[4] = {
-      c[0] * c[1] * c[2] + s[0] * s[1] * s[2],  // q0=cos(θ/2)
-      c[0] * c[1] * s[2] - s[0] * s[1] * c[2],  // q1=rx*sin(θ/2)
-      c[0] * s[1] * c[2] + s[0] * c[1] * s[2],  // q2=ry*sin(θ/2)
-      s[0] * c[1] * c[2] - c[0] * s[1] * s[2]   // q3=rz*sin(θ/2)
-  };
-  return (Matrixf<4, 1>(q) / Matrixf<4, 1>(q).norm());
-}
-
-Matrixf<4, 1> robotics::quat2angvec(Matrixf<4, 1> q) {
-  float cosq0;
-  float theta = 2.0f * acosf(math::limit(q[0][0], -1, 1));
-  if (theta == 0 || theta == PI) {
-    float angvec[4] = {0, 0, 0, theta};  // θ=0||PI, return[0;θ]
-    return Matrixf<4, 1>(angvec);
-  }
-  Matrixf<3, 1> vec = q.block<3, 1>(1, 0);
-  float angvec[4] = {
-      vec[0][0] / vec.norm(),  // rx
-      vec[1][0] / vec.norm(),  // ry
-      vec[2][0] / vec.norm(),  // rz
-      theta                    // theta
-  };
-  return Matrixf<4, 1>(angvec);
-}
-
-Matrixf<4, 1> robotics::angvec2quat(Matrixf<4, 1> angvec) {
-  float c = cosf(0.5f * angvec[3][0]), s = sinf(0.5f * angvec[3][0]);
-  float q[4] = {
-      c,                 // q0=cos(θ/2)
-      s * angvec[0][0],  // q1=rx*sin(θ/2)
-      s * angvec[1][0],  // q2=ry*sin(θ/2)
-      s * angvec[2][0]   // q3=rz*sin(θ/2)
-  };
-  return Matrixf<4, 1>(q) / Matrixf<4, 1>(q).norm();
-}
-
-Matrixf<3, 3> robotics::t2r(Matrixf<4, 4> T) {
-  return T.block<3, 3>(0, 0);  // R=T(1:3,1:3)
-}
-
-Matrixf<4, 4> robotics::r2t(Matrixf<3, 3> R) {
-  // T=[R,0;0,1]
-  float T[16] = {R[0][0], R[0][1], R[0][2], 0, R[1][0], R[1][1], R[1][2], 0,
-                 R[2][0], R[2][1], R[2][2], 0, 0,       0,       0,       1};
-  return Matrixf<4, 4>(T);
-}
-
-Matrixf<3, 1> robotics::t2p(Matrixf<4, 4> T) {
-  return T.block<3, 1>(0, 3);  // p=T(1:3,4)
-}
-
-Matrixf<4, 4> robotics::p2t(Matrixf<3, 1> p) {
-  // T=[I,P;0,1]
-  float T[16] = {1, 0, 0, p[0][0], 0, 1, 0, p[1][0],
-                 0, 0, 1, p[2][0], 0, 0, 0, 1};
-  return Matrixf<4, 4>(T);
-}
-
-Matrixf<4, 4> robotics::rp2t(Matrixf<3, 3> R, Matrixf<3, 1> p) {
-  // T=[R,P;0,1]
-  float T[16] = {R[0][0], R[0][1], R[0][2], p[0][0], R[1][0], R[1][1],
-                 R[1][2], p[1][0], R[2][0], R[2][1], R[2][2], p[2][0],
-                 0,       0,       0,       1};
-  return Matrixf<4, 4>(T);
-}
-
-Matrixf<4, 4> robotics::invT(Matrixf<4, 4> T) {
-  Matrixf<3, 3> RT = t2r(T).trans();
-  Matrixf<3, 1> p_ = -1.0f * RT * t2p(T);
-  float invT[16] = {RT[0][0], RT[0][1], RT[0][2], p_[0][0], RT[1][0], RT[1][1],
-                    RT[1][2], p_[1][0], RT[2][0], RT[2][1], RT[2][2], p_[2][0],
-                    0,        0,        0,        1};
-  return Matrixf<4, 4>(invT);
-}
-
-Matrixf<3, 1> robotics::t2rpy(Matrixf<4, 4> T) {
-  return r2rpy(t2r(T));
-}
-
-Matrixf<4, 4> robotics::rpy2t(Matrixf<3, 1> rpy) {
-  return r2t(rpy2r(rpy));
-}
-
-Matrixf<4, 1> robotics::t2angvec(Matrixf<4, 4> T) {
-  return r2angvec(t2r(T));
-}
-
-Matrixf<4, 4> robotics::angvec2t(Matrixf<4, 1> angvec) {
-  return r2t(angvec2r(angvec));
-}
-
-Matrixf<4, 1> robotics::t2quat(Matrixf<4, 4> T) {
-  return r2quat(t2r(T));
-}
-
-Matrixf<4, 4> robotics::quat2t(Matrixf<4, 1> quat) {
-  return r2t(quat2r(quat));
-}
-
-Matrixf<6, 1> robotics::t2twist(Matrixf<4, 4> T) {
-  Matrixf<3, 1> p = t2p(T);
-  Matrixf<4, 1> angvec = t2angvec(T);
-  Matrixf<3, 1> phi = angvec.block<3, 1>(0, 0) * angvec[3][0];
-  float twist[6] = {p[0][0], p[1][0], p[2][0], phi[0][0], phi[1][0], phi[2][0]};
-  return Matrixf<6, 1>(twist);
-}
-
-Matrixf<4, 4> robotics::twist2t(Matrixf<6, 1> twist) {
-  Matrixf<3, 1> p = twist.block<3, 1>(0, 0);
-  float theta = twist.block<3, 1>(3, 0).norm();
-  float angvec[4] = {0, 0, 0, theta};
-  if (theta != 0) {
-    angvec[0] = twist[3][0] / theta;
-    angvec[1] = twist[4][0] / theta;
-    angvec[2] = twist[5][0] / theta;
-  }
-  return rp2t(angvec2r(angvec), p);
-}
-
-Matrixf<4, 4> robotics::DH_t::fkine() {
-  float ct = cosf(theta), st = sinf(theta);  // cosθ, sinθ
-  float ca = cosf(alpha), sa = sinf(alpha);  // cosα, sinα
-
-  // T =
-  // | cθ  -sθcα   sθsα   acθ |
-  // | sθ   cθcα  -cθsα   asθ |
-  // |  0     sα     cα     d |
-  // |  0      0      0     1 |
-  T[0][0] = ct;
-  T[0][1] = -st * ca;
-  T[0][2] = st * sa;
-  T[0][3] = a * ct;
-
-  T[1][0] = st;
-  T[1][1] = ct * ca;
-  T[1][2] = -ct * sa;
-  T[1][3] = a * st;
-
-  T[2][0] = 0;
-  T[2][1] = sa;
-  T[2][2] = ca;
-  T[2][3] = d;
-
-  T[3][0] = 0;
-  T[3][1] = 0;
-  T[3][2] = 0;
-  T[3][3] = 1;
-
-  return T;
-}
-
-robotics::Link::Link(float theta,
-                     float d,
-                     float a,
-                     float alpha,
-                     robotics::Joint_Type_e type,
-                     float offset,
-                     float qmin,
-                     float qmax,
-                     float m,
-                     Matrixf<3, 1> rc,
-                     Matrixf<3, 3> I) {
-  dh_.theta = theta;
-  dh_.d = d;
-  dh_.alpha = alpha;
-  dh_.a = a;
-  type_ = type;
-  offset_ = offset;
-  qmin_ = qmin;
-  qmax_ = qmax;
-  m_ = m;
-  rc_ = rc;
-  I_ = I;
-}
-
-robotics::Link::Link(const Link& link) {
-  dh_.theta = link.dh_.theta;
-  dh_.d = link.dh_.d;
-  dh_.alpha = link.dh_.alpha;
-  dh_.a = link.dh_.a;
-  type_ = link.type_;
-  offset_ = link.offset_;
-  qmin_ = link.qmin_;
-  qmax_ = link.qmax_;
-  m_ = link.m_;
-  rc_ = link.rc_;
-  I_ = link.I_;
-}
-
-robotics::Link& robotics::Link::operator=(Link link) {
-  dh_.theta = link.dh_.theta;
-  dh_.d = link.dh_.d;
-  dh_.alpha = link.dh_.alpha;
-  dh_.a = link.dh_.a;
-  type_ = link.type_;
-  offset_ = link.offset_;
-  qmin_ = link.qmin_;
-  qmax_ = link.qmax_;
-  m_ = link.m_;
-  rc_ = link.rc_;
-  I_ = link.I_;
-  return *this;
-}
-
-Matrixf<4, 4> robotics::Link::T(float q) {
-  if (type_ == R) {
-    if (qmin_ >= qmax_)
-      dh_.theta = q + offset_;
-    else
-      dh_.theta = math::limit(q + offset_, qmin_, qmax_);
-  } else {
-    if (qmin_ >= qmax_)
-      dh_.d = q + offset_;
-    else
-      dh_.d = math::limit(q + offset_, qmin_, qmax_);
-  }
-  return dh_.fkine();
-}

modules/robotics/robotics.h

@@ -1,407 +0,0 @@
-/**
- ******************************************************************************
- * @file    robotics.cpp/h
- * @brief   Robotic toolbox on STM32. STM32机器人学库
- * @author  Spoon Guan
- * @ref     [1] SJTU ME385-2, Robotics, Y.Ding
- *          [2] Bruno Siciliano, et al., Robotics: Modelling, Planning and
- *              Control, Springer, 2010.
- *          [3] R.Murry, Z.X.Li, and S.Sastry, A Mathematical Introduction
- *              to Robotic Manipulation, CRC Press, 1994.
- ******************************************************************************
- * Copyright (c) 2023 Team JiaoLong-SJTU
- * All rights reserved.
- ******************************************************************************
- */
-
-#ifndef ROBOTICS_H
-#define ROBOTICS_H
-
-#include "utils.h"
-#include "matrix.h"
-
-namespace robotics {
-// rotation matrix(R) -> RPY([yaw;pitch;roll])
-Matrixf<3, 1> r2rpy(Matrixf<3, 3> R);
-// RPY([yaw;pitch;roll]) -> rotation matrix(R)
-Matrixf<3, 3> rpy2r(Matrixf<3, 1> rpy);
-// rotation matrix(R) -> angle vector([r;θ])
-Matrixf<4, 1> r2angvec(Matrixf<3, 3> R);
-// angle vector([r;θ]) -> rotation matrix(R)
-Matrixf<3, 3> angvec2r(Matrixf<4, 1> angvec);
-// rotation matrix(R) -> quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)]
-Matrixf<4, 1> r2quat(Matrixf<3, 3> R);
-// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> rotation matrix(R)
-Matrixf<3, 3> quat2r(Matrixf<4, 1> quat);
-// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> RPY([yaw;pitch;roll])
-Matrixf<3, 1> quat2rpy(Matrixf<4, 1> q);
-// RPY([yaw;pitch;roll]) -> quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)]
-Matrixf<4, 1> rpy2quat(Matrixf<3, 1> rpy);
-// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> angle vector([r;θ])
-Matrixf<4, 1> quat2angvec(Matrixf<4, 1> q);
-// angle vector([r;θ]) -> quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)]
-Matrixf<4, 1> angvec2quat(Matrixf<4, 1> angvec);
-// homogeneous transformation matrix(T) -> rotation matrix(R)
-Matrixf<3, 3> t2r(Matrixf<4, 4> T);
-// rotation matrix(R) -> homogeneous transformation matrix(T)
-Matrixf<4, 4> r2t(Matrixf<3, 3> R);
-// homogeneous transformation matrix(T) -> translation vector(p)
-Matrixf<3, 1> t2p(Matrixf<4, 4> T);
-// translation vector(p) -> homogeneous transformation matrix(T)
-Matrixf<4, 4> p2t(Matrixf<3, 1> p);
-// rotation matrix(R) & translation vector(p) -> homogeneous transformation
-// matrix(T)
-Matrixf<4, 4> rp2t(Matrixf<3, 3> R, Matrixf<3, 1> p);
-// homogeneous transformation matrix(T) -> RPY([yaw;pitch;roll])
-Matrixf<3, 1> t2rpy(Matrixf<4, 4> T);
-// inverse of homogeneous transformation matrix(T^-1=[R',-R'P;0,1])
-Matrixf<4, 4> invT(Matrixf<4, 4> T);
-// RPY([yaw;pitch;roll]) -> homogeneous transformation matrix(T)
-Matrixf<4, 4> rpy2t(Matrixf<3, 1> rpy);
-// homogeneous transformation matrix(T) -> angle vector([r;θ])
-Matrixf<4, 1> t2angvec(Matrixf<4, 4> T);
-// angle vector([r;θ]) -> homogeneous transformation matrix(T)
-Matrixf<4, 4> angvec2t(Matrixf<4, 1> angvec);
-// homogeneous transformation matrix(T) -> quaternion,
-// [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)]
-Matrixf<4, 1> t2quat(Matrixf<4, 4> T);
-// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> homogeneous transformation
-// matrix(T)
-Matrixf<4, 4> quat2t(Matrixf<4, 1> quat);
-// homogeneous transformation matrix(T) -> twist coordinates vector ([p;rθ])
-Matrixf<6, 1> t2twist(Matrixf<4, 4> T);
-// twist coordinates vector ([p;rθ]) -> homogeneous transformation matrix(T)
-Matrixf<4, 4> twist2t(Matrixf<6, 1> twist);
-
-// joint type: R-revolute joint, P-prismatic joint
-typedef enum joint_type {
-  R = 0,
-  P = 1,
-} Joint_Type_e;
-
-// Denavit–Hartenberg(DH) method
-struct DH_t {
-  // forward kinematic
-  Matrixf<4, 4> fkine();
-  // DH parameter
-  float theta;
-  float d;
-  float a;
-  float alpha;
-  Matrixf<4, 4> T;
-};
-
-class Link {
- public:
-  Link(){};
-  Link(float theta, float d, float a, float alpha, Joint_Type_e type = R,
-       float offset = 0, float qmin = 0, float qmax = 0, float m = 1,
-       Matrixf<3, 1> rc = matrixf::zeros<3, 1>(),
-       Matrixf<3, 3> I = matrixf::zeros<3, 3>());
-  Link(const Link& link);
-
-  Link& operator=(Link link);
-
-  float qmin() { return qmin_; }
-  float qmax() { return qmax_; }
-  Joint_Type_e type() { return type_; }
-  float m() { return m_; }
-  Matrixf<3, 1> rc() { return rc_; }
-  Matrixf<3, 3> I() { return I_; }
-
-  Matrixf<4, 4> T(float q);  // forward kinematic
-
- public:
-  // kinematic parameter
-  DH_t dh_;
-  float offset_;
-  // limit(qmin,qmax), no limit if qmin<=qmax
-  float qmin_;
-  float qmax_;
-  // joint type
-  Joint_Type_e type_;
-  // dynamic parameter
-  float m_;           // mass
-  Matrixf<3, 1> rc_;  // centroid(link coordinate)
-  Matrixf<3, 3> I_;   // inertia tensor(3*3)
-};
-
-template <uint16_t _n = 1>
-class Serial_Link {
- public:
-  Serial_Link(Link links[_n]) {
-    for (int i = 0; i < _n; i++)
-      links_[i] = links[i];
-    gravity_ = matrixf::zeros<3, 1>();
-    gravity_[2][0] = -9.81f;
-  }
-
-  Serial_Link(Link links[_n], Matrixf<3, 1> gravity) {
-    for (int i = 0; i < _n; i++)
-      links_[i] = links[i];
-    gravity_ = gravity;
-  }
-
-  // forward kinematic: T_n^0
-  // param[in] q: joint variable vector
-  // param[out] T_n^0
-  Matrixf<4, 4> fkine(Matrixf<_n, 1> q) {
-    T_ = matrixf::eye<4, 4>();
-    for (int iminus1 = 0; iminus1 < _n; iminus1++)
-      T_ = T_ * links_[iminus1].T(q[iminus1][0]);
-    return T_;
-  }
-
-  // forward kinematic: T_k^0
-  // param[in] q: joint variable vector
-  // param[in] k: joint number
-  // param[out] T_k^0
-  Matrixf<4, 4> fkine(Matrixf<_n, 1> q, uint16_t k) {
-    if (k > _n)
-      k = _n;
-    Matrixf<4, 4> T = matrixf::eye<4, 4>();
-    for (int iminus1 = 0; iminus1 < k; iminus1++)
-      T = T * links_[iminus1].T(q[iminus1][0]);
-    return T;
-  }
-
-  // T_k^k-1: homogeneous transformation matrix of link k
-  // param[in] q: joint variable vector
-  // param[in] kminus: joint number k, input k-1
-  // param[out] T_k^k-1
-  Matrixf<4, 4> T(Matrixf<_n, 1> q, uint16_t kminus1) {
-    if (kminus1 >= _n)
-      kminus1 = _n - 1;
-    return links_[kminus1].T(q[kminus1][0]);
-  }
-
-  // jacobian matrix, J_i = [J_pi;j_oi]
-  // param[in] q: joint variable vector
-  // param[out] jacobian matix J_6*n
-  Matrixf<6, _n> jacob(Matrixf<_n, 1> q) {
-    Matrixf<3, 1> p_e = t2p(fkine(q));               // p_e
-    Matrixf<4, 4> T_iminus1 = matrixf::eye<4, 4>();  // T_i-1^0
-    Matrixf<3, 1> z_iminus1;                         // z_i-1^0
-    Matrixf<3, 1> p_iminus1;                         // p_i-1^0
-    Matrixf<3, 1> J_pi;
-    Matrixf<3, 1> J_oi;
-    for (int iminus1 = 0; iminus1 < _n; iminus1++) {
-      // revolute joint: J_pi = z_i-1x(p_e-p_i-1), J_oi = z_i-1
-      if (links_[iminus1].type() == R) {
-        z_iminus1 = T_iminus1.block<3, 1>(0, 2);
-        p_iminus1 = t2p(T_iminus1);
-        T_iminus1 = T_iminus1 * links_[iminus1].T(q[iminus1][0]);
-        J_pi = vector3f::cross(z_iminus1, p_e - p_iminus1);
-        J_oi = z_iminus1;
-      }
-      // prismatic joint: J_pi = z_i-1, J_oi = 0
-      else {
-        z_iminus1 = T_iminus1.block<3, 1>(0, 2);
-        T_iminus1 = T_iminus1 * links_[iminus1].T(q[iminus1][0]);
-        J_pi = z_iminus1;
-        J_oi = matrixf::zeros<3, 1>();
-      }
-      J_[0][iminus1] = J_pi[0][0];
-      J_[1][iminus1] = J_pi[1][0];
-      J_[2][iminus1] = J_pi[2][0];
-      J_[3][iminus1] = J_oi[0][0];
-      J_[4][iminus1] = J_oi[1][0];
-      J_[5][iminus1] = J_oi[2][0];
-    }
-    return J_;
-  }
-
-  // inverse kinematic, numerical solution(Newton method)
-  // param[in] T: homogeneous transformation matrix of end effector
-  // param[in] q: initial joint variable vector(q0) for Newton method's
-  //              iteration
-  // param[in] tol: tolerance of error (norm(error of twist vector))
-  // param[in] max_iter: maximum iterations, default 30
-  // param[out] q: joint variable vector
-  Matrixf<_n, 1> ikine(Matrixf<4, 4> Td,
-                       Matrixf<_n, 1> q = matrixf::zeros<_n, 1>(),
-                       float tol = 1e-4f, uint16_t max_iter = 50) {
-    Matrixf<4, 4> T;
-    Matrixf<3, 1> pe, we;
-    Matrixf<6, 1> err, new_err;
-    Matrixf<_n, 1> dq;
-    float step = 1;
-    for (int i = 0; i < max_iter; i++) {
-      T = fkine(q);
-      pe = t2p(Td) - t2p(T);
-      // angvec(Td*T^-1), transform angular vector(T->Td) in world coordinate
-      we = t2twist(Td * invT(T)).block<3, 1>(3, 0);
-      for (int i = 0; i < 3; i++) {
-        err[i][0] = pe[i][0];
-        err[i + 3][0] = we[i][0];
-      }
-      if (err.norm() < tol)
-        return q;
-      // adjust iteration step
-      Matrixf<6, _n> J = jacob(q);
-      for (int j = 0; j < 5; j++) {
-        dq = matrixf::inv(J.trans() * J) * (J.trans() * err) * step;
-        if (dq[0][0] == INFINITY)  // J'*J singular
-        {
-          dq = matrixf::inv(J.trans() * J + 0.1f * matrixf::eye<_n, _n>()) *
-               J.trans() * err * step;
-          // SVD<6, _n> JTJ_svd(J.trans() * J);
-          // dq = JTJ_svd.solve(err) * step * 5e-2f;
-          q += dq;
-          for (int i = 0; i < _n; i++) {
-            if (links_[i].type() == R)
-              q[i][0] = math::loopLimit(q[i][0], -PI, PI);
-          }
-          break;
-        }
-        T = fkine(q + dq);
-        pe = t2p(Td) - t2p(T);
-        we = t2twist(Td * invT(T)).block<3, 1>(3, 0);
-        for (int i = 0; i < 3; i++) {
-          new_err[i][0] = pe[i][0];
-          new_err[i + 3][0] = we[i][0];
-        }
-        if (new_err.norm() < err.norm()) {
-          q += dq;
-          for (int i = 0; i < _n; i++) {
-            if (links_[i].type() == robotics::Joint_Type_e::R) {
-              q[i][0] = math::loopLimit(q[i][0], -PI, PI);
-            }
-          }
-          break;
-        } else {
-          step /= 2.0f;
-        }
-      }
-      if (step < 1e-3f)
-        return q;
-    }
-    return q;
-  }
-
-  // (Reserved function) inverse kinematic, analytic solution(geometric method)
-  Matrixf<_n, 1> (*ikine_analytic)(Matrixf<4, 4> T);
-
-  // inverse dynamic, Newton-Euler method
-  // param[in]  q: joint variable vector
-  // param[in]  qv: dq/dt
-  // param[in]  qa: d^2q/dt^2
-  // param[in]  he: load on end effector [f;μ], default 0
-  Matrixf<_n, 1> rne(Matrixf<_n, 1> q,
-                     Matrixf<_n, 1> qv = matrixf::zeros<_n, 1>(),
-                     Matrixf<_n, 1> qa = matrixf::zeros<_n, 1>(),
-                     Matrixf<6, 1> he = matrixf::zeros<6, 1>()) {
-    // forward propagation
-    // record each links' motion state in matrices
-    // [ωi] angular velocity
-    Matrixf<3, _n + 1> w = matrixf::zeros<3, _n + 1>();
-    // [βi] angular acceleration
-    Matrixf<3, _n + 1> b = matrixf::zeros<3, _n + 1>();
-    // [pi] position of joint
-    Matrixf<3, _n + 1> p = matrixf::zeros<3, _n + 1>();
-    // [vi] velocity of joint
-    Matrixf<3, _n + 1> v = matrixf::zeros<3, _n + 1>();
-    // [ai] acceleration of joint
-    Matrixf<3, _n + 1> a = matrixf::zeros<3, _n + 1>();
-    // [aci] acceleration of mass center
-    Matrixf<3, _n + 1> ac = matrixf::zeros<3, _n + 1>();
-    // temperary vectors
-    Matrixf<3, 1> w_i, b_i, p_i, v_i, ai, ac_i;
-    // i & i-1 coordinate convert to 0 coordinate
-    Matrixf<4, 4> T_0i = matrixf::eye<4, 4>();
-    Matrixf<4, 4> T_0iminus1 = matrixf::eye<4, 4>();
-    Matrixf<3, 3> R_0i = matrixf::eye<3, 3>();
-    Matrixf<3, 3> R_0iminus1 = matrixf::eye<3, 3>();
-    // unit vector of z-axis
-    Matrixf<3, 1> ez = matrixf::zeros<3, 1>();
-    ez[2][0] = 1;
-
-    for (int i = 1; i <= _n; i++) {
-      T_0i = T_0i * T(q, i - 1);     // T_i^0
-      R_0i = t2r(T_0i);              // R_i^0
-      R_0iminus1 = t2r(T_0iminus1);  // R_i-1^0
-      // ω_i = ω_i-1+qv_i*R_i-1^0*ez
-      w_i = w.col(i - 1) + qv[i - 1][0] * R_0iminus1 * ez;
-      // β_i = β_i-1+ω_i-1x(qv_i*R_i-1^0*ez)+qa_i*R_i-1^0*ez
-      b_i = b.col(i - 1) +
-            vector3f::cross(w.col(i - 1), qv[i - 1][0] * R_0iminus1 * ez) +
-            qa[i - 1][0] * R_0iminus1 * ez;
-      p_i = t2p(T_0i);  // p_i = T_i^0(1:3,4)
-      // v_i = v_i-1+ω_ix(p_i-p_i-1)
-      v_i = v.col(i - 1) + vector3f::cross(w_i, p_i - p.col(i - 1));
-      // a_i = a_i-1+β_ix(p_i-p_i-1)+ω_ix(ω_ix(p_i-p_i-1))
-      ai = a.col(i - 1) + vector3f::cross(b_i, p_i - p.col(i - 1)) +
-           vector3f::cross(w_i, vector3f::cross(w_i, p_i - p.col(i - 1)));
-      // ac_i = a_i+β_ix(R_0^i*rc_i^i)+ω_ix(ω_ix(R_0^i*rc_i^i))
-      ac_i =
-          ai + vector3f::cross(b_i, R_0i * links_[i - 1].rc()) +
-          vector3f::cross(w_i, vector3f::cross(w_i, R_0i * links_[i - 1].rc()));
-      for (int row = 0; row < 3; row++) {
-        w[row][i] = w_i[row][0];
-        b[row][i] = b_i[row][0];
-        p[row][i] = p_i[row][0];
-        v[row][i] = v_i[row][0];
-        a[row][i] = ai[row][0];
-        ac[row][i] = ac_i[row][0];
-      }
-      T_0iminus1 = T_0i;  // T_i-1^0
-    }
-
-    // backward propagation
-    // record each links' force
-    Matrixf<3, _n + 1> f = matrixf::zeros<3, _n + 1>();   // joint force
-    Matrixf<3, _n + 1> mu = matrixf::zeros<3, _n + 1>();  // joint moment
-    // temperary vector
-    Matrixf<3, 1> f_iminus1, mu_iminus1;
-    // {T,R',P}_i^i-1
-    Matrixf<4, 4> T_iminus1i;
-    Matrixf<3, 3> RT_iminus1i;
-    Matrixf<3, 1> P_iminus1i;
-    // I_i-1(in 0 coordinate)
-    Matrixf<3, 3> I_i;
-    // joint torque
-    Matrixf<_n, 1> torq;
-
-    // load on end effector
-    for (int row = 0; row < 3; row++) {
-      f[row][_n] = he.block<3, 1>(0, 0)[row][0];
-      mu[row][_n] = he.block<3, 1>(3, 0)[row][0];
-    }
-    for (int i = _n; i > 0; i--) {
-      T_iminus1i = T(q, i - 1);               // T_i^i-1
-      P_iminus1i = t2p(T_iminus1i);           // P_i^i-1
-      RT_iminus1i = t2r(T_iminus1i).trans();  // R_i^i-1'
-      R_0iminus1 = R_0i * RT_iminus1i;        // R_i-1^0
-      // I_i^0 = R_i^0*I_i^i*(R_i^0)'
-      I_i = R_0i * links_[i - 1].I() * R_0i.trans();
-      // f_i-1 = f_i+m_i*ac_i-m_i*g
-      f_iminus1 = f.col(i) + links_[i - 1].m() * ac.col(i) -
-                  links_[i - 1].m() * gravity_;
-      // μ_i-1 = μ_i+f_ixrc_i-f_i-1xrc_i-1->ci+I_i*b_i+ω_ix(I_i*ω_i)
-      mu_iminus1 = mu.col(i) +
-                   vector3f::cross(f.col(i), R_0i * links_[i - 1].rc()) -
-                   vector3f::cross(f_iminus1, R_0i * (RT_iminus1i * P_iminus1i +
-                                                      links_[i - 1].rc())) +
-                   I_i * b.col(i) + vector3f::cross(w.col(i), I_i * w.col(i));
-      // τ_i = μ_i-1'*(R_i-1^0*ez)
-      torq[i - 1][0] = (mu_iminus1.trans() * R_0iminus1 * ez)[0][0];
-      for (int row = 0; row < 3; row++) {
-        f[row][i - 1] = f_iminus1[row][0];
-        mu[row][i - 1] = mu_iminus1[row][0];
-      }
-      R_0i = R_0iminus1;
-    }
-
-    return torq;
-  }
-
- private:
-  Link links_[_n];
-  Matrixf<3, 1> gravity_;
-
-  Matrixf<4, 4> T_;
-  Matrixf<6, _n> J_;
-};
-};  // namespace robotics
-
-#endif  // ROBOTICS_H

modules/robotics/utils.cpp

@@ -1,56 +0,0 @@
-/**
- ******************************************************************************
- * @file    utils.cpp/h
- * @brief   General math utils. 常用数学工具函数
- * @author  Spoon Guan
- ******************************************************************************
- * Copyright (c) 2023 Team JiaoLong-SJTU
- * All rights reserved.
- ******************************************************************************
- */
-
-#include "utils.h"
-
-float math::limit(float val, const float& min, const float& max) {
-  if (min > max)
-    return val;
-  else if (val < min)
-    val = min;
-  else if (val > max)
-    val = max;
-  return val;
-}
-
-float math::limitMin(float val, const float& min) {
-  if (val < min)
-    val = min;
-  return val;
-}
-
-float math::limitMax(float val, const float& max) {
-  if (val > max)
-    val = max;
-  return val;
-}
-
-float math::loopLimit(float val, const float& min, const float& max) {
-  if (min >= max)
-    return val;
-  if (val > max) {
-    while (val > max)
-      val -= (max - min);
-  } else if (val < min) {
-    while (val < min)
-      val += (max - min);
-  }
-  return val;
-}
-
-float math::sign(const float& val) {
-  if (val > 0)
-    return 1;
-  else if (val < 0)
-    return -1;
-  return 0;
-}
-

modules/robotics/utils.h

@@ -1,27 +0,0 @@
-/**
- ******************************************************************************
- * @file    utils.cpp/h
- * @brief   General math utils. 常用数学工具函数
- * @author  Spoon Guan
- ******************************************************************************
- * Copyright (c) 2023 Team JiaoLong-SJTU
- * All rights reserved.
- ******************************************************************************
- */
-
-#ifndef UTILS_H
-#define UTILS_H
-
-#include <stdint.h>
-
-namespace math {
-
-float limit(float val, const float& min, const float& max);
-float limitMin(float val, const float& min);
-float limitMax(float val, const float& max);
-float loopLimit(float val, const float& min, const float& max);
-float sign(const float& val);
-
-}  // namespace math
-
-#endif  // UTILS_H

modules/super_cap/super_cap.c

@@ -24,8 +24,7 @@ SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config)
 
     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_config_pid);
+    PIDInit(&super_cap_instance->buffer_pid, &supercap_config->buffer_pid_config);
     return super_cap_instance;
 }
 

modules/super_cap/super_cap.h

@@ -17,16 +17,16 @@ typedef struct
 /* 超级电容实例 */
 typedef struct
 {
-    CANInstance *can_ins;   // CAN实例
+    CANInstance *can_ins; // CAN实例
     SuperCap_Msg_s cap_msg; // 超级电容信息
-    PIDInstance buffer_pid; // 对缓冲功率进行闭环
+    PIDInstance buffer_pid; //缓冲功率闭环控制参数
 } SuperCapInstance;
 
 /* 超级电容初始化配置 */
 typedef struct
 {
     CAN_Init_Config_s can_config;
-    PID_Init_Config_s buffer_config_pid;
+    PID_Init_Config_s buffer_pid_config;
 } SuperCap_Init_Config_s;
 
 /**

modules/vofa/vofa.c

@@ -6,13 +6,6 @@
  * @LastEditTime: 2022-12-05 14:15:53
  */
 #include "vofa.h"
-#include "usbd_cdc_if.h"
-
-typedef union
-{
-    float float_t;
-    uint8_t uint8_t[4];
-} send_float;
 
 /*VOFA浮点协议*/
 void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart )
@@ -36,57 +29,5 @@ void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart
     send_data[4 * num + 2] = 0x80;
     send_data[4 * num + 3] = 0x7f; //加上协议要求的4个尾巴
 
-    //HAL_UART_Transmit(huart, (uint8_t *)send_data, 4 * num + 4, 100);
-    CDC_Transmit_FS((uint8_t *)send_data,4 * num + 4);
-}
-
-#define BYTE0(dwTemp) (*(char*)(&dwTemp))
-#define BYTE1(dwTemp) (*((char*)(&dwTemp)+1))
-#define BYTE2(dwTemp) (*((char*)(&dwTemp)+2))
-#define BYTE3(dwTemp) (*((char*)(&dwTemp)+3))
-
-uint8_t DataSendBuf[100];
-//匿名上位机
-void ANODT_SendF1(int32_t Angle,int32_t speed_rpm,int32_t Angle_target,int32_t speed_target)//F1灵活格式帧
-{
-    uint8_t cnt=0;
-    DataSendBuf[cnt++]=0xAA;  //帧头
-    DataSendBuf[cnt++]=0xFF;  //目标地址
-    DataSendBuf[cnt++]=0xF1;  //功能码
-    DataSendBuf[cnt++]=16;  //数据长度
-
-    DataSendBuf[cnt++]=BYTE0(Angle);
-    DataSendBuf[cnt++]=BYTE1(Angle);
-    DataSendBuf[cnt++]=BYTE2(Angle);
-    DataSendBuf[cnt++]=BYTE3(Angle);
-
-    DataSendBuf[cnt++]=BYTE0(speed_rpm);
-    DataSendBuf[cnt++]=BYTE1(speed_rpm);
-    DataSendBuf[cnt++]=BYTE2(speed_rpm);
-    DataSendBuf[cnt++]=BYTE3(speed_rpm);
-
-    DataSendBuf[cnt++]=BYTE0(Angle_target);
-    DataSendBuf[cnt++]=BYTE1(Angle_target);
-    DataSendBuf[cnt++]=BYTE2(Angle_target);
-    DataSendBuf[cnt++]=BYTE3(Angle_target);
-
-    DataSendBuf[cnt++]=BYTE0(speed_target);
-    DataSendBuf[cnt++]=BYTE1(speed_target);
-    DataSendBuf[cnt++]=BYTE2(speed_target);
-    DataSendBuf[cnt++]=BYTE3(speed_target);
-
-    uint8_t sc=0;  //和校验
-    uint8_t ac=0;  //附加校验
-    for(uint8_t i=0;i<DataSendBuf[3]+4;i++)
-    {
-        sc+=DataSendBuf[i];
-        ac+=sc;
-    }
-    DataSendBuf[cnt++]=sc;
-    DataSendBuf[cnt++]=ac;
-
-//	for(uint8_t i=0;i<cnt;i++)
-//		HAL_UART_Transmit(&huart6,&DataSendBuf[i],1,100);
-    CDC_Transmit_FS(DataSendBuf, cnt);
-
+    HAL_UART_Transmit(huart, (uint8_t *)send_data, 4 * num + 4, 100);
 }

modules/vofa/vofa.h

@@ -11,7 +11,11 @@
 #include "bsp_usart.h"
 #include "usart.h"
 
-
+typedef union
+{
+    float float_t;
+    uint8_t uint8_t[4];
+} send_float;
 void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart);
-void ANODT_SendF1(int32_t Angle,int32_t speed_rpm,int32_t Angle_target,int32_t speed_target);
+
 #endif // !1#define