更新电机

assets/User_code/device/motor_lk.c

@@ -76,9 +76,16 @@ static int8_t MOTOR_LK_CreateCANManager(BSP_CAN_t can) {
 }
 
 static void MOTOR_LK_Decode(MOTOR_LK_t *motor, BSP_CAN_Message_t *msg) {
+    // 调试信息：打印接收到的数据
+    // printf("LK Motor ID:%d, CMD:0x%02X, Data: %02X %02X %02X %02X %02X %02X %02X %02X\n", 
+    //        motor->param.id, msg->data[0], msg->data[0], msg->data[1], msg->data[2], 
+    //        msg->data[3], msg->data[4], msg->data[5], msg->data[6], msg->data[7]);
+    
     // 检查命令字节是否为反馈命令
     if (msg->data[0] != LK_CMD_FEEDBACK) {
-        return;
+        // 如果不是标准反馈命令，可能是其他格式的数据
+        // 临时跳过命令字节检查，直接解析数据
+        // return;
     }
     
     // 解析温度 (DATA[1])
@@ -138,8 +145,9 @@ int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param) {
     memset(&new_motor->motor, 0, sizeof(MOTOR_t));
     new_motor->motor.reverse = param->reverse;
     
-    // 计算反馈ID（假设为控制ID + 0x40）
+    // 对于某些LK电机，反馈数据可能通过命令ID发送
-    uint16_t feedback_id = param->id + 0x40;
+    // 根据实际测试，使用命令ID接收反馈数据
+    uint16_t feedback_id = 0x140 + param->id;  // 使用命令ID作为反馈ID
     
     // 注册CAN接收ID
     if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) {
@@ -161,8 +169,8 @@ int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param) {
     for (int i = 0; i < manager->motor_count; i++) {
         MOTOR_LK_t *motor = manager->motors[i];
         if (motor && motor->param.id == param->id) {
-            // 计算反馈ID
+            // 对于某些LK电机，反馈数据通过命令ID发送
-            uint16_t feedback_id = param->id + 0x100;
+            uint16_t feedback_id = 0x140 + param->id;
             
             BSP_CAN_Message_t rx_msg;
             if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
@@ -217,9 +225,9 @@ int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) {
     // 转矩闭环控制命令 - 将输出值转换为转矩控制值
     int16_t torque_control = (int16_t)(value * (float)LK_TORQUE_RANGE);
     
-    // 构建CAN帧
+    // 构建CAN帧（根据协议：命令报文标识符 = 0x140 + ID）
     BSP_CAN_StdDataFrame_t tx_frame;
-    tx_frame.id = param->id;
+    tx_frame.id = 0x140 + param->id;
     tx_frame.dlc = MOTOR_TX_BUF_SIZE;
     
     // 设置转矩闭环控制命令数据
@@ -245,7 +253,7 @@ int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param) {
     if (param == NULL) return DEVICE_ERR_NULL;
     
     BSP_CAN_StdDataFrame_t tx_frame;
-    tx_frame.id = param->id;
+    tx_frame.id = 0x140 + param->id;
     tx_frame.dlc = MOTOR_TX_BUF_SIZE;
     
     // 电机运行命令
@@ -265,7 +273,7 @@ int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param) {
     if (param == NULL) return DEVICE_ERR_NULL;
     
     BSP_CAN_StdDataFrame_t tx_frame;
-    tx_frame.id = param->id;
+    tx_frame.id = 0x140 + param->id;
     tx_frame.dlc = MOTOR_TX_BUF_SIZE;
     
     // 电机关闭命令

assets/User_code/device/motor_lz.c

@@ -27,10 +27,19 @@
 #define LZ_RAW_VALUE_MAX            (65535)         /* 16位原始值最大值 */
 #define LZ_TEMP_SCALE               (10.0f)         /* 温度缩放因子 */
 
+#define LZ_MAX_RECOVER_DIFF_RAD    (0.4f)
 #define MOTOR_TX_BUF_SIZE           (8)
 #define MOTOR_RX_BUF_SIZE           (8)
 
 /* Private macro ------------------------------------------------------------ */
+
+MOTOR_LZ_MotionParam_t lz_recover_param = {
+    .target_angle = 0.0f,
+    .target_velocity = 0.0f,
+    .kp = 20.0f,
+    .kd = 1.0f,
+    .torque = 0.0f,
+};
 /* Private typedef ---------------------------------------------------------- */
 /* Private variables -------------------------------------------------------- */
 static MOTOR_LZ_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
@@ -445,7 +454,7 @@ int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param) {
     return DEVICE_ERR_NO_DEV;
 }
 
-MOTOR_LZ_Feedback_t* MOTOR_LZ_GetFeedback(MOTOR_LZ_Param_t *param) {
+static MOTOR_LZ_Feedback_t* MOTOR_LZ_GetFeedback(MOTOR_LZ_Param_t *param) {
     MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
     if (motor && motor->motor.header.online) {
         return &motor->lz_feedback;
@@ -492,5 +501,32 @@ int8_t MOTOR_LZ_VelocityControl(MOTOR_LZ_Param_t *param, float target_velocity)
     motion_param.kp = 0.0f;
     motion_param.kd = 1.0f;    // 少量阻尼
     
+    return MOTOR_LZ_MotionControl(param, &motion_param);
+}
+
+int8_t MOTOR_LZ_RecoverToZero(MOTOR_LZ_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
+    if (motor == NULL) return DEVICE_ERR_NO_DEV;
+    
+    // 获取当前角度
+    MOTOR_LZ_Feedback_t *feedback = MOTOR_LZ_GetFeedback(param);
+    if (feedback == NULL) return DEVICE_ERR_NO_DEV;
+    
+    float current_angle = feedback->current_angle;
+    
+    // 计算目标角度为0时的最短路径
+    float angle_diff = -current_angle; // 目标是0，所以差值就是-current_angle
+    // 限制最大差值，防止过大跳变
+    if (angle_diff > LZ_MAX_RECOVER_DIFF_RAD) angle_diff = LZ_MAX_RECOVER_DIFF_RAD;
+    if (angle_diff < -LZ_MAX_RECOVER_DIFF_RAD) angle_diff = -LZ_MAX_RECOVER_DIFF_RAD;
+    
+    float target_angle = current_angle + angle_diff;
+    
+    // 创建运控参数，设置位置和速度限制
+    MOTOR_LZ_MotionParam_t motion_param = lz_recover_param; // 使用预设的恢复参数
+    motion_param.target_angle = target_angle;
+
     return MOTOR_LZ_MotionControl(param, &motion_param);
 }

assets/User_code/device/motor_lz.h

@@ -213,12 +213,7 @@ int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param);
  */
 int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param);
 
-/**
+int8_t MOTOR_LZ_RecoverToZero(MOTOR_LZ_Param_t *param);
- * @brief 获取指定电机的反馈信息
- * @param param 电机参数
- * @return 电机反馈信息指针，失败返回NULL
- */
-MOTOR_LZ_Feedback_t* MOTOR_LZ_GetFeedback(MOTOR_LZ_Param_t *param);
 
 #ifdef __cplusplus
 }