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

修改out

Browse Source
This commit is contained in:
Robofish 2025-10-05 17:23:03 +08:00
parent 50775af3b0
commit 7f1c8f38b4
1 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
User/module/balance_chassis.c
View File

@ -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
}
}