diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 8ca3e2f..6da7985 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -6,6 +6,7 @@ #include "module/balance_chassis.h" #include "bsp/can.h" #include "component/user_math.h" +#include "device/motor_lk.h" #include "module/config.h" #include #include @@ -14,8 +15,8 @@ /* Private define ----------------------------------------------------------- */ #define CAN_CMD_ENABLE_ID 121 // 使能命令ID #define CAN_CMD_JOINT_ID 122 // 关节控制命令ID -#define CAN_CMD_WHEEL_LEFT_ID 128 // 左轮控制命令ID -#define CAN_CMD_WHEEL_RIGHT_ID 129 // 右轮控制命令ID +#define CAN_CMD_WHEEL_LEFT_ID 0x128 // 左轮控制命令ID +#define CAN_CMD_WHEEL_RIGHT_ID 0x129 // 右轮控制命令ID #define CAN_FEEDBACK_JOINT_BASE_ID 124 // 关节反馈基础ID (124-127) #define CAN_FEEDBACK_WHEEL_LEFT_ID 130 // 左轮反馈ID @@ -71,25 +72,16 @@ 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; - // 直接转发CAN数据到左轮电机 - BSP_CAN_StdDataFrame_t wheel_frame = { - .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); + 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); + } // 检查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; - // 直接转发CAN数据到右轮电机 - BSP_CAN_StdDataFrame_t wheel_frame = { - .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); + 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); } return DEVICE_OK;