robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

添加超时

Browse Source
This commit is contained in:
Robofish 2025-10-05 17:10:43 +08:00
parent 6035580f27
commit 50775af3b0
1 changed files with 18 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
User/module/balance_chassis.c
View File

@ -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]);
}
}