From 485e25fec252ad3be27a93a7db2f526b6b792e6a Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Mon, 1 Sep 2025 00:02:40 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E7=94=B5=E6=9C=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- assets/User_code/device/motor_lk.c | 26 +++++++++++++------- assets/User_code/device/motor_lz.c | 38 +++++++++++++++++++++++++++++- assets/User_code/device/motor_lz.h | 7 +----- 3 files changed, 55 insertions(+), 16 deletions(-) diff --git a/assets/User_code/device/motor_lk.c b/assets/User_code/device/motor_lk.c index 0349984..d145868 100644 --- a/assets/User_code/device/motor_lk.c +++ b/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） - uint16_t feedback_id = param->id + 0x40; + // 对于某些LK电机，反馈数据可能通过命令ID发送 + // 根据实际测试，使用命令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 - uint16_t feedback_id = param->id + 0x100; + // 对于某些LK电机，反馈数据通过命令ID发送 + 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; // 电机关闭命令 diff --git a/assets/User_code/device/motor_lz.c b/assets/User_code/device/motor_lz.c index 0e20443..45f7f76 100644 --- a/assets/User_code/device/motor_lz.c +++ b/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); } \ No newline at end of file diff --git a/assets/User_code/device/motor_lz.h b/assets/User_code/device/motor_lz.h index cd58ae9..8bc3bd1 100644 --- a/assets/User_code/device/motor_lz.h +++ b/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); -/** - * @brief 获取指定电机的反馈信息 - * @param param 电机参数 - * @return 电机反馈信息指针，失败返回NULL - */ -MOTOR_LZ_Feedback_t* MOTOR_LZ_GetFeedback(MOTOR_LZ_Param_t *param); +int8_t MOTOR_LZ_RecoverToZero(MOTOR_LZ_Param_t *param); #ifdef __cplusplus }