增加了电机前馈控制,更新readme，加快ins速度，删除了工程和平衡底盘相关的app

.vscode/tasks.json

@@ -5,7 +5,7 @@
         {
             "label": "build task",         // 任务标签
             "type": "shell",               // 任务类型,因为要调用mingw32-make,是在终端(CMD)里运行的,所以是shell任务
-            "command": "mingw32-make -j24 -l12",// 任务命令,线程数可以根据自己的电脑修改,建议为核数的4~8倍
+            "command": "mingw32-make -j24 -l12",// 任务命令,线程数可以根据自己的电脑修改,建议与cpu核数相同
             "problemMatcher": [],          
             "group": {
                 "kind": "build",

Makefile

@@ -156,7 +156,6 @@ application/gimbal/gimbal.c \
 application/chassis/chassis.c \
 application/shoot/shoot.c \
 application/cmd/robot_cmd.c \
-application/balance_chassis/balance.c \
 application/robot.c
 
 # ASM sources
@@ -235,7 +234,6 @@ C_INCLUDES =  \
 -Iapplication/shoot \
 -Iapplication/gimbal \
 -Iapplication/cmd \
--Iapplication/balance_chassis \
 -Iapplication \
 -Ibsp/dwt \
 -Ibsp/can \

Src/freertos.c

@@ -170,7 +170,7 @@ void StartINSTASK(void const *argument)
   {
     // 1kHz
     INS_Task();
-    VisionSend(); // 解算完成后发送视觉数据
+    VisionSend(); // 解算完æˆ<EFBFBD>å<EFBFBD>Žå<EFBFBD>‘é€<EFBFBD>视觉数æ<EFBFBD>?
     osDelay(1);
   }
 }

Src/spi.c

@@ -49,7 +49,7 @@ void MX_SPI1_Init(void)
   hspi1.Init.CLKPolarity = SPI_POLARITY_HIGH;
   hspi1.Init.CLKPhase = SPI_PHASE_2EDGE;
   hspi1.Init.NSS = SPI_NSS_SOFT;
-  hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_256;
+  hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8;
   hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
   hspi1.Init.TIMode = SPI_TIMODE_DISABLE;
   hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;

application/grab/grab.md

@@ -1 +0,0 @@
-工程机器人夹爪  

application/lift/lift.md

@@ -1,3 +0,0 @@
-抬升/横移机构
-
-气缸和电磁阀似乎没有必要构建模块,但是也可以组合微动开关和编码器+继电器等传感器构成模块,降低lift的复杂度,完成解耦.

application/manipulator/manipulator.md

@@ -1 +0,0 @@
-机械臂(不包含末端执行机构)

application/robot_def.h

@@ -25,16 +25,6 @@
 #define VISION_USE_VCP  // 使用虚拟串口发送视觉数据
 // #define VISION_USE_UART // 使用串口发送视觉数据
 
-// @todo: 增加机器人类型定义,后续是否要兼容所有机器人?(只兼容步兵英雄哨兵似乎就够了)
-// 通过该宏,你可以直接将所有机器人的参数保存在一处,然后每次只需要修改这个宏就可以替换所有参数
-/* 机器人类型定义 */
-// #define ROBOT_HERO 1     // 英雄机器人
-// #define ROBOT_ENINEER 2  // 工程机器人
-#define ROBOT_INFANTRY 3 // 步兵机器人3
-// #define ROBOT_INFANTRY 4 // 步兵机器人4
-// #define ROBOT_INFANTRY 5 // 步兵机器人5
-// #define ROBOT_SENTRY 6   // 哨兵机器人
-
 /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
 // 云台参数
 #define YAW_CHASSIS_ALIGN_ECD 2711  // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
@@ -188,7 +178,6 @@ typedef struct
  *
  */
 
-/* @todo : 对于平衡底盘,需要新增控制模式和控制数据 */
 typedef struct
 {
 #if defined(CHASSIS_BOARD) || defined(GIMBAL_BOARD) // 非单板的时候底盘还将imu数据回传(若有必要)
@@ -205,7 +194,7 @@ typedef struct
 
 } Chassis_Upload_Data_s;
 
-/* @todo : 对于平衡底盘,需要不同的反馈数据 */
+
 typedef struct
 {
     attitude_t gimbal_imu_data;

basic_framework.ioc

@@ -520,7 +520,7 @@ ProjectManager.StackSize=0x4000
 ProjectManager.TargetToolchain=Makefile
 ProjectManager.ToolChainLocation=
 ProjectManager.UnderRoot=false
-ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_DMA_Init-DMA-false-HAL-true,3-SystemClock_Config-RCC-false-HAL-false,4-MX_ADC1_Init-ADC1-false-HAL-true,5-MX_CAN1_Init-CAN1-false-HAL-true,6-MX_CAN2_Init-CAN2-false-HAL-true,7-MX_SPI1_Init-SPI1-false-HAL-true,8-MX_TIM4_Init-TIM4-false-HAL-true,9-MX_TIM5_Init-TIM5-false-HAL-true,10-MX_USART3_UART_Init-USART3-false-HAL-true,11-MX_RNG_Init-RNG-false-HAL-true,12-MX_RTC_Init-RTC-false-HAL-true,13-MX_TIM1_Init-TIM1-false-HAL-true,14-MX_TIM10_Init-TIM10-false-HAL-true,15-MX_USART1_UART_Init-USART1-false-HAL-true,16-MX_USART6_UART_Init-USART6-false-HAL-true,17-MX_TIM8_Init-TIM8-false-HAL-true,18-MX_I2C2_Init-I2C2-false-HAL-true,19-MX_I2C3_Init-I2C3-false-HAL-true,20-MX_SPI2_Init-SPI2-false-HAL-true,21-MX_CRC_Init-CRC-false-HAL-true,22-MX_DAC_Init-DAC-false-HAL-true,23-MX_USB_OTG_FS_USB_Init-USB_OTG_FS-false-HAL-true
+ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_DMA_Init-DMA-false-HAL-true,3-SystemClock_Config-RCC-false-HAL-false,4-MX_ADC1_Init-ADC1-false-HAL-true,5-MX_CAN1_Init-CAN1-false-HAL-true,6-MX_CAN2_Init-CAN2-false-HAL-true,7-MX_SPI1_Init-SPI1-false-HAL-true,8-MX_TIM4_Init-TIM4-false-HAL-true,9-MX_TIM5_Init-TIM5-false-HAL-true,10-MX_USART3_UART_Init-USART3-false-HAL-true,11-MX_RNG_Init-RNG-false-HAL-true,12-MX_RTC_Init-RTC-false-HAL-true,13-MX_TIM1_Init-TIM1-false-HAL-true,14-MX_TIM10_Init-TIM10-false-HAL-true,15-MX_USART1_UART_Init-USART1-false-HAL-true,16-MX_USART6_UART_Init-USART6-false-HAL-true,17-MX_TIM8_Init-TIM8-false-HAL-true,18-MX_I2C2_Init-I2C2-false-HAL-true,19-MX_I2C3_Init-I2C3-false-HAL-true,20-MX_SPI2_Init-SPI2-false-HAL-true,21-MX_CRC_Init-CRC-false-HAL-true,22-MX_DAC_Init-DAC-false-HAL-true,23-MX_USB_DEVICE_Init-USB_DEVICE-false-HAL-false
 RCC.48MHZClocksFreq_Value=48000000
 RCC.AHBFreq_Value=168000000
 RCC.APB1CLKDivider=RCC_HCLK_DIV4
@@ -591,10 +591,10 @@ SH.S_TIM8_CH2.0=TIM8_CH2,Output Compare2 CH2
 SH.S_TIM8_CH2.ConfNb=1
 SH.S_TIM8_CH3.0=TIM8_CH3,Output Compare3 CH3
 SH.S_TIM8_CH3.ConfNb=1
-SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_256
+SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_8
 SPI1.CLKPhase=SPI_PHASE_2EDGE
 SPI1.CLKPolarity=SPI_POLARITY_HIGH
-SPI1.CalculateBaudRate=328.125 KBits/s
+SPI1.CalculateBaudRate=10.5 MBits/s
 SPI1.Direction=SPI_DIRECTION_2LINES
 SPI1.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,BaudRatePrescaler,CLKPolarity,CLKPhase
 SPI1.Mode=SPI_MODE_MASTER

bsp/bsp_init.c

@@ -11,7 +11,6 @@ void BSPInit()
 {
     DWT_Init(168);
     BSPLogInit();
-    // USBInit(); // 务必在进入操作系统之前执行USBInit
 
 
     // legacy support，待删除,将在实现了led/tempctrl/buzzer的module之后移动到app层进行XXXRegister()

bsp/can/bsp_can.c

@@ -3,6 +3,7 @@
 #include "memory.h"
 #include "stdlib.h"
 #include "bsp_dwt.h"
+#include "bsp_log.h"
 
 /* can instance ptrs storage, used for recv callback */
 // 在CAN产生接收中断会遍历数组,选出hcan和rxid与发生中断的实例相同的那个,调用其回调函数
@@ -30,9 +31,9 @@ static void CANAddFilter(CANInstance *_instance)
     CAN_FilterTypeDef can_filter_conf;
     static uint8_t can1_filter_idx = 0, can2_filter_idx = 14; // 0-13给can1用,14-27给can2用
 
-    can_filter_conf.FilterMode = CAN_FILTERMODE_IDLIST;  //使用id list模式,即只有将rxid添加到过滤器中才会接收到,其他报文会被过滤
+    can_filter_conf.FilterMode = CAN_FILTERMODE_IDLIST;                                                       // 使用id list模式,即只有将rxid添加到过滤器中才会接收到,其他报文会被过滤
-    can_filter_conf.FilterScale = CAN_FILTERSCALE_16BIT; //使用16位id模式,即只有低16位有效
+    can_filter_conf.FilterScale = CAN_FILTERSCALE_16BIT;                                                      // 使用16位id模式,即只有低16位有效
-    can_filter_conf.FilterFIFOAssignment = (_instance->tx_id & 1) ? CAN_RX_FIFO0 : CAN_RX_FIFO1; //奇数id的模块会被分配到FIFO0,偶数id的模块会被分配到FIFO1
+    can_filter_conf.FilterFIFOAssignment = (_instance->tx_id & 1) ? CAN_RX_FIFO0 : CAN_RX_FIFO1;              // 奇数id的模块会被分配到FIFO0,偶数id的模块会被分配到FIFO1
     can_filter_conf.SlaveStartFilterBank = 14;                                                                // 从第14个过滤器开始配置从机过滤器(在STM32的BxCAN控制器中CAN2是CAN1的从机)
     can_filter_conf.FilterIdLow = _instance->rx_id << 5;                                                      // 过滤器寄存器的低16位,因为使用STDID,所以只有低11位有效,高5位要填0
     can_filter_conf.FilterBank = _instance->can_handle == &hcan1 ? (can1_filter_idx++) : (can2_filter_idx++); // 根据can_handle判断是CAN1还是CAN2,然后自增
@@ -97,18 +98,28 @@ CANInstance *CANRegister(CAN_Init_Config_s *config)
 
 /* @todo 目前似乎封装过度,应该添加一个指向tx_buff的指针,tx_buff不应该由CAN instance保存 */
 /* 如果让CANinstance保存txbuff,会增加一次复制的开销 */
-uint8_t CANTransmit(CANInstance *_instance,uint8_t timeout)
+uint8_t CANTransmit(CANInstance *_instance, float timeout)
 {
+    static uint32_t busy_count;
+    static float wait_time;
     float dwt_start = DWT_GetTimeline_ms();
     while (HAL_CAN_GetTxMailboxesFreeLevel(_instance->can_handle) == 0) // 等待邮箱空闲
     {
         if (DWT_GetTimeline_ms() - dwt_start > timeout) // 超时
         {
+            LOGWARNING("CAN BUSY sending! cnt:%d", busy_count);
+            busy_count++;
             return 0;
         }
     }
+    wait_time = DWT_GetTimeline_ms() - dwt_start;
     // tx_mailbox会保存实际填入了这一帧消息的邮箱,但是知道是哪个邮箱发的似乎也没啥用
-    HAL_CAN_AddTxMessage(_instance->can_handle, &_instance->txconf, _instance->tx_buff, &_instance->tx_mailbox);
+    if (HAL_CAN_AddTxMessage(_instance->can_handle, &_instance->txconf, _instance->tx_buff, &_instance->tx_mailbox))
+    {
+        LOGWARNING("CAN BUSY bus! cnt:%d", busy_count);
+        busy_count++;
+        return 0;
+    }
     return 1; // 发送成功
 }
 
@@ -177,6 +188,4 @@ void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan)
     CANFIFOxCallback(hcan, CAN_RX_FIFO1); // 调用我们自己写的函数来处理消息
 }
 
-
 // 人工智能会毁灭人类吗？请在补全回答
-

bsp/can/bsp_can.h

@@ -63,6 +63,6 @@ void CANSetDLC(CANInstance *_instance, uint8_t length);
  * @param timeout 超时时间,单位为ms;后续改为us,获得更精确的控制
  * @param _instance* can instance owned by module
  */
-uint8_t CANTransmit(CANInstance *_instance,uint8_t timeout);
+uint8_t CANTransmit(CANInstance *_instance,float timeout);
 
 #endif

bsp/usb/bsp_usb.h

@@ -2,7 +2,6 @@
  * @file bsp_usb.h
  * @author your name (you@domain.com)
  * @brief 提供对usb vpc(virtal com port)的操作接口,hid和msf考虑后续添加
- * @todo 与usart的接口统一完成兼容
  * @attention 这一版usb修改了usbd_cdc_if.c中的CDC_Receive_FS函数,若使用cube生成后会被覆盖.后续需要由usbcdciftemplate创建一套自己的模板
  * @version 0.1
  * @date 2023-02-09

modules/BMI088/bmi088.c

@@ -227,7 +227,7 @@ static void BMI088GyroINTCallback(GPIOInstance *gpio)
 
 /**
  * @brief
- * @todo 现在要考虑一下数据返回的方式,指针还是结构体? 7个float数据有点费时,不然用DMA? or memcpy
+ * @todo 现在要考虑一下数据返回的方式,指针还是结构体?
  *
  * @param bmi088
  * @return BMI088_Data_t

modules/BMI088/bmi088.h

@@ -100,7 +100,6 @@ BMI088Instance *BMI088Register(BMI088_Init_Config_s *config);
 
 /**
  * @brief 读取BMI088数据
- * @todo  7个float数据开销较大,后续考虑使用DMA或双缓冲区直接传递指针
  * @param bmi088 BMI088实例指针
  * @return BMI088_Data_t 读取到的数据
  */

modules/algorithm/algorithm.md

@@ -3,11 +3,8 @@
 <p align='right'>neozng1@hnu.edu.cn</p>
 
 > TODO:
->
 > 1. 实现麦轮和全向轮的速度解算
-> 2. 实现LQR
+> 2. 实现一些通用的滤波器，如指数平均，窗平均，低通等
-> 3. 实现一些通用的滤波器，如指数平均，窗平均，低通等
-> 4. 实现系统辨识
 
 ## 总览和使用
 

modules/algorithm/controller.c

@@ -134,6 +134,7 @@ void PIDInit(PIDInstance *pid, PID_Init_Config_s *config)
     // utilize the quality of struct that its memeory is continuous
     memcpy(pid, config, sizeof(PID_Init_Config_s));
     // set rest of memory to 0
+    DWT_GetDeltaT(&pid->DWT_CNT);
 }
 
 /**

modules/daemon/daemon.c

@@ -13,7 +13,7 @@ DaemonInstance *DaemonRegister(Daemon_Init_Config_s *config)
     memset(instance, 0, sizeof(DaemonInstance));
 
     instance->owner_id = config->owner_id;
-    instance->reload_count = config->reload_count;
+    instance->reload_count = config->reload_count == 0 ? 100 : config->reload_count;
     instance->callback = config->callback;
 
     daemon_instances[idx++] = instance;

modules/master_machine/master_process.md

@@ -94,7 +94,6 @@ typedef struct
 	float yaw;
 	float pitch;
 	float roll;
-	// uint32_t time_stamp; // @todo 用于和相机的时间戳对齐
 } Vision_Send_s;
 ```
 

modules/message_center/message_center.h

@@ -2,7 +2,6 @@
  * @file message_center.h
  * @author NeoZeng neozng1@hnu.edu.cn
  * @brief 这是一个伪pubsub机制,仅对应用之间的通信进行了隔离
- * @todo 实现基于链表-队列的pubsub机制
  * @version 0.1
  * @date 2022-11-30
  *

modules/motor/DJImotor/dji_motor.c

@@ -160,6 +160,8 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
     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;
     instance->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
+    instance->motor_controller.current_feedforward_ptr = config->controller_param_init_config.current_feedforward_ptr;
+    instance->motor_controller.speed_feedforward_ptr = config->controller_param_init_config.speed_feedforward_ptr;
     // 后续增加电机前馈控制器(速度和电流)
 
     // 电机分组,因为至多4个电机可以共用一帧CAN控制报文
@@ -251,6 +253,9 @@ void DJIMotorControl()
         // 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
         if ((motor_setting->close_loop_type & SPEED_LOOP) && (motor_setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
         {
+            if (motor_setting->feedforward_flag & SPEED_FEEDFORWARD)
+                pid_ref += *motor_controller->speed_feedforward_ptr;
+
             if (motor_setting->speed_feedback_source == OTHER_FEED)
                 pid_measure = *motor_controller->other_speed_feedback_ptr;
             else // MOTOR_FEED
@@ -260,6 +265,8 @@ 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_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);

modules/motor/DJImotor/dji_motor.h

@@ -7,7 +7,6 @@
  *
  * @todo  1. 给不同的电机设置不同的低通滤波器惯性系数而不是统一使用宏
           2. 为M2006和M3508增加开环的零位校准函数,并在初始化时调用(根据用户配置决定是否调用)
-          3. 完成前馈功能（已经添加了前馈数据指针）
 
  * @copyright Copyright (c) 2022 HNU YueLu EC all rights reserved
  *
@@ -34,7 +33,7 @@ typedef struct
     uint16_t last_ecd;        // 上一次读取的编码器值
     uint16_t ecd;             // 0-8191,刻度总共有8192格
     float angle_single_round; // 单圈角度
-    float speed_aps;          // 角速度,单位为:度/秒 rpm:rounds per minute
+    float speed_aps;          // 角速度,单位为:度/秒
     int16_t real_current;     // 实际电流
     uint8_t temperate;        // 温度 Celsius
 
@@ -100,7 +99,6 @@ void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback
 
 /**
  * @brief 该函数被motor_task调用运行在rtos上,motor_stask内通过osDelay()确定控制频率
- * @todo  增加前馈功能
  */
 void DJIMotorControl();
 

modules/motor/HTmotor/HT04.c

@@ -1,6 +1,7 @@
 #include "HT04.h"
 #include "memory.h"
 #include "general_def.h"
+#include "bsp_dwt.h"
 
 static uint8_t idx;
 HTMotorInstance *ht_motor_instance[HT_MOTOR_CNT];
@@ -43,19 +44,27 @@ static void HTMotorDecode(CANInstance *motor_can)
     uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
     uint8_t *rxbuff = motor_can->rx_buff;
     HTMotor_Measure_t *measure = &((HTMotorInstance *)motor_can->id)->measure; // 将can实例中保存的id转换成电机实例的指针
+    measure->feedback_dt = DWT_GetDeltaT(&measure->count);
 
     measure->last_angle = measure->total_angle;
-
     tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
-    measure->total_angle =  uint_to_float(tmp, P_MIN, P_MAX, 16);
+    measure->total_angle = uint_to_float(tmp, P_MIN, P_MAX, 16) - measure->angle_bias;
 
-    tmp = (uint16_t)((rxbuff[3] << 4) | (rxbuff[4] >> 4));
+    measure->speed_rads = // SPEED_SMOOTH_COEF * measure->speed_rads + (1 - SPEED_SMOOTH_COEF) *
-    measure->speed_rads = SPEED_SMOOTH_COEF * uint_to_float(tmp, V_MIN, V_MAX, 12) +
+        (measure->total_angle - measure->last_angle) / measure->feedback_dt;
-                          (1 - SPEED_SMOOTH_COEF) * measure->speed_rads;
 
-    tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
+    measure->real_current = uint_to_float(((uint16_t)(rxbuff[3] << 4) | (uint16_t)(rxbuff[4] >> 4)), V_MIN, V_MAX, 12);
-    measure->real_current = CURRENT_SMOOTH_COEF * uint_to_float(tmp, T_MIN, T_MAX, 12) +
+
-                            (1 - CURRENT_SMOOTH_COEF) * measure->real_current;
+    if (!measure->first_feedback_flag) // 初始化的时候设置偏置
+    {
+        measure->first_feedback_flag = 1;
+        measure->angle_bias = measure->total_angle;
+        measure->total_angle = 0;
+        measure->speed_rads = 0;
+        DWT_GetDeltaT(&measure->count);
+    }
+
+    // measure->real_current = uint_to_float((rxbuff[3] << 8) | rxbuff[4], I_MIN, I_MAX, 16);
 }
 
 HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config)
@@ -81,7 +90,7 @@ HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config)
 
 void HTMotorSetRef(HTMotorInstance *motor, float ref)
 {
-    motor->pid_ref = ref;
+    motor->pid_ref = ref * 0.285f;
 }
 
 void HTMotorControl()
@@ -107,7 +116,7 @@ void HTMotorControl()
             if (setting->angle_feedback_source == OTHER_FEED)
                 pid_measure = *motor->other_angle_feedback_ptr;
             else
-                pid_measure = measure->real_current;
+                pid_measure = measure->total_angle;
             // measure单位是rad,ref是角度,统一到angle下计算,方便建模
             pid_ref = PIDCalculate(&motor->angle_PID, pid_measure * RAD_2_DEGREE, pid_ref);
         }
@@ -125,11 +134,10 @@ void HTMotorControl()
             pid_ref = PIDCalculate(&motor->speed_PID, pid_measure * RAD_2_DEGREE, pid_ref);
         }
 
-        if (setting->close_loop_type & CURRENT_LOOP)
-        {
         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);
         }
 
@@ -139,30 +147,34 @@ void HTMotorControl()
 
         LIMIT_MIN_MAX(set, T_MIN, T_MAX);           // 限幅,实际上这似乎和pid输出限幅重复了
         tmp = float_to_uint(set, T_MIN, T_MAX, 12); // 数值最后在 -12~+12之间
+        if (motor->stop_flag == MOTOR_STOP)
+            tmp = float_to_uint(0, T_MIN, T_MAX, 12);
         motor_can->tx_buff[6] = (tmp >> 8);
         motor_can->tx_buff[7] = tmp & 0xff;
 
-        if (motor->stop_flag == MOTOR_STOP)
-        { // 若该电机处于停止状态,直接将发送buff置零
-            memset(motor_can->tx_buff + 6, 0, sizeof(uint16_t));
-        }
         CANTransmit(motor_can, 1);
     }
 }
 
 void HTMotorStop(HTMotorInstance *motor)
 {
+    if (motor->stop_flag == MOTOR_STOP)
+        return;
     HTMotorSetMode(CMD_RESET_MODE, motor);
+    HTMotorSetMode(CMD_RESET_MODE, motor); // 发两次,确保电机停止
     motor->stop_flag = MOTOR_STOP;
 }
 
 void HTMotorEnable(HTMotorInstance *motor)
 {
+    if (motor->stop_flag == MOTOR_ENALBED)
+        return;
+    HTMotorSetMode(CMD_MOTOR_MODE, motor);
     HTMotorSetMode(CMD_MOTOR_MODE, motor);
     motor->stop_flag = MOTOR_ENALBED;
 }
 
-void HTMotorCalibEncoder(HTMotorInstance *motor)
+// void HTMotorCalibEncoder(HTMotorInstance *motor)
-{
+// {
-    HTMotorSetMode(CMD_ZERO_POSITION, motor);
+//     HTMotorSetMode(CMD_ZERO_POSITION, motor);
-}
+// }

modules/motor/HTmotor/HT04.h

@@ -19,10 +19,15 @@
 
 typedef struct // HT04
 {
-    float last_angle;
     float total_angle; // 角度为多圈角度,范围是-95.5~95.5,单位为rad
+    float last_angle;
     float speed_rads;
     float real_current;
+    float angle_bias;
+    uint8_t first_feedback_flag;
+
+    float feedback_dt;
+    uint32_t count;
 } HTMotor_Measure_t;
 
 /* HT电机类型定义*/
@@ -91,11 +96,7 @@ void HTMotorStop(HTMotorInstance *motor);
 void HTMotorEnable(HTMotorInstance *motor);
 
 /**
- * @brief 校准电机编码器
+ * @brief 校准电机编码器,设置的当前位置为0
- * @attention 注意,校准时务必将电机和其他机构分离,电机会旋转360°!
- *            注意,校准时务必将电机和其他机构分离,电机会旋转360°!
- *            注意,校准时务必将电机和其他机构分离,电机会旋转360°!
- *            注意,校准时务必将电机和其他机构分离,电机会旋转360°!
  *
  * @param motor
  */

modules/motor/LKmotor/LK9025.c

@@ -104,10 +104,10 @@ void LKMotorControl()
             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->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);

modules/motor/motor_def.h

@@ -45,7 +45,7 @@ typedef enum
 /* 反馈来源设定,若设为OTHER_FEED则需要指定数据来源指针,详见Motor_Controller_s*/
 typedef enum
 {
-    MOTOR_FEED=0,
+    MOTOR_FEED = 0,
     OTHER_FEED,
 } Feedback_Source_e;
 
@@ -87,8 +87,8 @@ typedef struct
 {
     float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针
     float *other_speed_feedback_ptr;
-    // float *speed_foward_ptr;
+    float *speed_feedforward_ptr;
-    // float *current_foward_ptr;
+    float *current_feedforward_ptr;
 
     PIDInstance current_PID;
     PIDInstance speed_PID;

modules/remote/remote_control.c

@@ -117,7 +117,6 @@ static void RemoteControlRxCallback()
  */
 static void RCLostCallback(void *id)
 {
-    // @todo 遥控器丢失的处理
     USARTServiceInit(rc_usart_instance); // 尝试重新启动接收
 }
 
@@ -130,7 +129,6 @@ RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle)
     rc_usart_instance = USARTRegister(&conf);
 
     // 进行守护进程的注册,用于定时检查遥控器是否正常工作
-    // @todo 当前守护进程直接在这里注册,后续考虑将其封装到遥控器的初始化函数中,即可以让用户决定reload_count的值(是否有必要?)
     Daemon_Init_Config_s daemon_conf = {
         .reload_count = 10, // 100ms未收到数据视为离线,遥控器的接收频率实际上是1000/14Hz(大约70Hz)
         .callback = RCLostCallback,