diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 5e13d9b..483b238 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -59,13 +59,15 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) { 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; memcpy(&torque_raw, &rx_msg.data[i * 2], sizeof(int16_t)); float torque = (float)torque_raw / 100.0f; // 转换为浮点数力矩值 - + chassis->output.joint[i] = torque; // 使用运控模式控制电机,只设置力矩,其他参数为0 MOTOR_LZ_MotionParam_t motion_param = { .target_angle = 0.0f, @@ -83,6 +85,7 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) { 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); } @@ -92,6 +95,7 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) { 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(400); // 确保CAN总线有足够时间处理消息 @@ -327,6 +331,8 @@ int8_t Chassis_Ctrl(Chassis_t *chassis) { (current_time - joint_last_cmd_time) > CMD_TIMEOUT_MS) { for (int i = 0; i < 4; i++) { MOTOR_LZ_Relax(&chassis->param.joint_param[i]); + + chassis->output.joint[i] = 0.0f; // 松弛时输出力矩设为0 } } @@ -335,6 +341,8 @@ int8_t Chassis_Ctrl(Chassis_t *chassis) { if (!wheel_command_received[i] && (current_time - wheel_last_cmd_time[i]) > CMD_TIMEOUT_MS) { MOTOR_LK_Relax(&chassis->param.wheel_param[i]); + + chassis->output.wheel[i] = 0.0f; // 松弛时输出设为0 } }