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

修lk

Browse Source
This commit is contained in:
Robofish 2025-10-05 14:43:31 +08:00
parent 5f33aac541
commit 555317ea3b
1 changed files with 8 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
User/module/balance_chassis.c
View File

@ -6,6 +6,7 @@
#include "module/balance_chassis.h" #include "module/balance_chassis.h"
#include "bsp/can.h" #include "bsp/can.h"
#include "component/user_math.h" #include "component/user_math.h"
#include "device/motor_lk.h"
#include "module/config.h" #include "module/config.h"
#include <string.h> #include <string.h>
#include <math.h> #include <math.h>
@ -14,8 +15,8 @@
/* Private define ----------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */
#define CAN_CMD_ENABLE_ID 121 // 使能命令ID #define CAN_CMD_ENABLE_ID 121 // 使能命令ID
#define CAN_CMD_JOINT_ID 122 // 关节控制命令ID #define CAN_CMD_JOINT_ID 122 // 关节控制命令ID
#define CAN_CMD_WHEEL_LEFT_ID 128 // 左轮控制命令ID #define CAN_CMD_WHEEL_LEFT_ID 0x128 // 左轮控制命令ID
#define CAN_CMD_WHEEL_RIGHT_ID 129 // 右轮控制命令ID #define CAN_CMD_WHEEL_RIGHT_ID 0x129 // 右轮控制命令ID
#define CAN_FEEDBACK_JOINT_BASE_ID 124 // 关节反馈基础ID (124-127) #define CAN_FEEDBACK_JOINT_BASE_ID 124 // 关节反馈基础ID (124-127)
#define CAN_FEEDBACK_WHEEL_LEFT_ID 130 // 左轮反馈ID #define CAN_FEEDBACK_WHEEL_LEFT_ID 130 // 左轮反馈ID
@ -71,25 +72,16 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) {
// 检查ID 128 - 左轮控制命令 (与电机发送格式一致) // 检查ID 128 - 左轮控制命令 (与电机发送格式一致)
if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_WHEEL_LEFT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { 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_command_received[0] = true;
// 直接转发CAN数据到左轮电机 float left_out = (float)((int16_t)(rx_msg.data[4] | (rx_msg.data[5] << 8))) / 2048.0f;
BSP_CAN_StdDataFrame_t wheel_frame = { MOTOR_LK_SetOutput(&chassis->param.wheel_param[0], left_out);
.id = chassis->param.wheel_param[0].id,
.dlc = rx_msg.dlc,
};
memcpy(wheel_frame.data, rx_msg.data, rx_msg.dlc);
BSP_CAN_TransmitStdDataFrame(chassis->param.wheel_param[0].can, &wheel_frame);
} }
// 检查ID 129 - 右轮控制命令 (与电机发送格式一致) // 检查ID 129 - 右轮控制命令 (与电机发送格式一致)
if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_WHEEL_RIGHT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { 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_command_received[1] = true;
// 直接转发CAN数据到右轮电机 float right_out = (float)((int16_t)(rx_msg.data[4] | (rx_msg.data[5] << 8))) / 2048.0f;
BSP_CAN_StdDataFrame_t wheel_frame = { MOTOR_LK_SetOutput(&chassis->param.wheel_param[1], right_out);
.id = chassis->param.wheel_param[1].id,
.dlc = rx_msg.dlc,
};
memcpy(wheel_frame.data, rx_msg.data, rx_msg.dlc);
BSP_CAN_TransmitStdDataFrame(chassis->param.wheel_param[1].can, &wheel_frame);
} }
return DEVICE_OK; return DEVICE_OK;