From 50775af3b0d60dff25fe86d369ab086b69ad9102 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 5 Oct 2025 17:10:43 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E8=B6=85=E6=97=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 0b63802..5e13d9b 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -24,10 +24,16 @@ #define CAN_FEEDBACK_WHEEL_RIGHT_ID 131 // 右轮反馈ID /* Private macro ------------------------------------------------------------ */ +#define CMD_TIMEOUT_MS 50 // 50ms超时时间，允许控制频率在10-20Hz之间 + /* Private variables -------------------------------------------------------- */ static bool joint_command_received = false; static bool wheel_command_received[2] = {false, false}; +// 超时管理 - 防止控制频率不同导致的控制/relax交替 +static uint32_t joint_last_cmd_time = 0; +static uint32_t wheel_last_cmd_time[2] = {0, 0}; + /* Private function --------------------------------------------------------- */ /** @@ -52,6 +58,8 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) { // 检查ID 122 - 运控模式控制4个关节电机 if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_JOINT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { joint_command_received = true; + joint_last_cmd_time = BSP_TIME_Get_ms(); // 更新关节命令时间戳 + // 8字节数据分别是4个电机的力矩 (每个电机2字节，有符号整数，精度0.01 Nm) for (int i = 0; i < 4; i++) { int16_t torque_raw; @@ -73,6 +81,7 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) { // 检查ID 128 - 左轮控制命令 (与电机发送格式一致) if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_WHEEL_LEFT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { wheel_command_received[0] = true; + wheel_last_cmd_time[0] = BSP_TIME_Get_ms(); // 更新左轮命令时间戳 float left_out = (float)((int16_t)(rx_msg.data[4] | (rx_msg.data[5] << 8))) / 2048.0f; MOTOR_LK_SetOutput(&chassis->param.wheel_param[0], left_out); @@ -81,6 +90,7 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) { // 检查ID 129 - 右轮控制命令 (与电机发送格式一致) if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_WHEEL_RIGHT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { wheel_command_received[1] = true; + wheel_last_cmd_time[1] = BSP_TIME_Get_ms(); // 更新右轮命令时间戳 float right_out = (float)((int16_t)(rx_msg.data[4] | (rx_msg.data[5] << 8))) / 2048.0f; MOTOR_LK_SetOutput(&chassis->param.wheel_param[1], right_out); } @@ -310,16 +320,20 @@ int8_t Chassis_Ctrl(Chassis_t *chassis) { // 处理CAN控制命令 Chassis_ProcessCANCommands(chassis); - // 如果没有收到关节控制命令，关节电机进入relax模式 - if (!joint_command_received) { + uint32_t current_time = BSP_TIME_Get_ms(); + + // 关节电机超时检查 - 只有在超时时才执行relax + if (!joint_command_received && + (current_time - joint_last_cmd_time) > CMD_TIMEOUT_MS) { for (int i = 0; i < 4; i++) { MOTOR_LZ_Relax(&chassis->param.joint_param[i]); } } - // 如果没有收到轮子控制命令，轮子电机进入relax模式 + // 轮子电机超时检查 - 只有在超时时才执行relax for (int i = 0; i < 2; i++) { - if (!wheel_command_received[i]) { + if (!wheel_command_received[i] && + (current_time - wheel_last_cmd_time[i]) > CMD_TIMEOUT_MS) { MOTOR_LK_Relax(&chassis->param.wheel_param[i]); } }