diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 2b19c33..b15ff36 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -46,7 +46,29 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) { joint_command_received = false; wheel_command_received[0] = false; wheel_command_received[1] = false; + + // 检查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; + chassis->output.wheel[0] = left_out; + MOTOR_LK_SetOutput(&chassis->param.wheel_param[0], left_out); + + } + // 检查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; + chassis->output.wheel[1] = right_out; + MOTOR_LK_SetOutput(&chassis->param.wheel_param[1], right_out); + } + + BSP_TIME_Delay_ms(1); // 短暂延时,避免总线冲突 + + // 检查ID 121 - 使能4个关节电机 if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_ENABLE_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { joint_command_received = true; @@ -79,26 +101,7 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) { MOTOR_LZ_MotionControl(&chassis->param.joint_param[i], &motion_param); } } - BSP_TIME_Delay_us(500); // 确保CAN总线有足够时间处理消息 - // 检查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; - chassis->output.wheel[0] = left_out; - MOTOR_LK_SetOutput(&chassis->param.wheel_param[0], left_out); - } - - // 检查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; - chassis->output.wheel[1] = right_out; - MOTOR_LK_SetOutput(&chassis->param.wheel_param[1], right_out); - } - BSP_TIME_Delay_us(500); return DEVICE_OK; }