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 21:05:17 +08:00
parent 600568fcff
commit 676d877d24
1 changed files with 22 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
User/module/balance_chassis.c
View File

@ -46,7 +46,29 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) {
joint_command_received = false; joint_command_received = false;
wheel_command_received[0] = false; wheel_command_received[0] = false;
wheel_command_received[1] = 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个关节电机 // 检查ID 121 - 使能4个关节电机
if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_ENABLE_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_ENABLE_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
joint_command_received = true; 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); 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; return DEVICE_OK;
} }