diff --git a/CMakeLists.txt b/CMakeLists.txt index b18dc79..1002f02 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -65,7 +65,8 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE User/device/motor_lk.c User/device/motor_lz.c - # User/module sources + # User/module + User/module/balance_chassis.c User/module/config.c # User/task sources diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c new file mode 100644 index 0000000..8ca3e2f --- /dev/null +++ b/User/module/balance_chassis.c @@ -0,0 +1,339 @@ +/* + * Balance Chassis Module + */ + +/* Includes ----------------------------------------------------------------- */ +#include "module/balance_chassis.h" +#include "bsp/can.h" +#include "component/user_math.h" +#include "module/config.h" +#include +#include + +/* Private typedef ---------------------------------------------------------- */ +/* 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_FEEDBACK_JOINT_BASE_ID 124 // 关节反馈基础ID (124-127) +#define CAN_FEEDBACK_WHEEL_LEFT_ID 130 // 左轮反馈ID +#define CAN_FEEDBACK_WHEEL_RIGHT_ID 131 // 右轮反馈ID + +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +static bool joint_command_received = false; +static bool wheel_command_received[2] = {false, false}; + +/* Private function --------------------------------------------------------- */ + +/** + * @brief 检查并处理CAN控制命令 + * @param chassis 底盘实例 + * @return 成功返回DEVICE_OK + */ +static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) { + BSP_CAN_Message_t rx_msg; + joint_command_received = false; + wheel_command_received[0] = false; + wheel_command_received[1] = false; + + // 检查ID 121 - 使能4个关节电机 + if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_ENABLE_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + joint_command_received = true; + for (int i = 0; i < 4; i++) { + MOTOR_LZ_Enable(&chassis->param.joint_param[i]); + } + } + + // 检查ID 122 - 运控模式控制4个关节电机 + if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_JOINT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + joint_command_received = true; + // 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; // 转换为浮点数力矩值 + + // 使用运控模式控制电机,只设置力矩,其他参数为0 + MOTOR_LZ_MotionParam_t motion_param = { + .target_angle = 0.0f, + .target_velocity = 0.0f, + .kp = 0.0f, + .kd = 0.0f, + .torque = torque + }; + MOTOR_LZ_MotionControl(&chassis->param.joint_param[i], &motion_param); + } + } + + // 检查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); + } + + // 检查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); + } + + return DEVICE_OK; +} + +/** + * @brief 发送关节电机反馈数据 + * @param chassis 底盘实例 + * @return 成功返回DEVICE_OK + */ +static int8_t Chassis_SendJointFeedback(Chassis_t *chassis) { + // 发送4个关节电机的反馈数据,ID分别为124、125、126、127 + for (int i = 0; i < 4; i++) { + MOTOR_LZ_t* motor = MOTOR_LZ_GetMotor(&chassis->param.joint_param[i]); + if (motor != NULL) { + BSP_CAN_StdDataFrame_t motor_frame = { + .id = CAN_FEEDBACK_JOINT_BASE_ID + i, // ID: 124, 125, 126, 127 + .dlc = 8, + .data = {0} + }; + + // 数据重构:转矩电流(2字节) + 位置(3字节) + 速度(3字节) = 8字节 + // 转矩电流 - 转换为16位整数 (精度0.01 Nm) + int16_t torque_int = (int16_t)(motor->lz_feedback.current_torque * 100); + memcpy(&motor_frame.data[0], &torque_int, sizeof(int16_t)); + + // 位置 - 转换为24位整数,使用3字节 (精度0.0001 rad) + int32_t angle_int = (int32_t)(motor->lz_feedback.current_angle * 10000) & 0xFFFFFF; + motor_frame.data[2] = (angle_int >> 16) & 0xFF; + motor_frame.data[3] = (angle_int >> 8) & 0xFF; + motor_frame.data[4] = angle_int & 0xFF; + + // 速度 - 转换为24位整数,使用3字节 (精度0.001 rad/s) + int32_t velocity_int = (int32_t)(motor->lz_feedback.current_velocity * 1000) & 0xFFFFFF; + motor_frame.data[5] = (velocity_int >> 16) & 0xFF; + motor_frame.data[6] = (velocity_int >> 8) & 0xFF; + motor_frame.data[7] = velocity_int & 0xFF; + + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &motor_frame); + } + } + + return DEVICE_OK; +} + +/** + * @brief 发送轮子电机反馈数据 + * @param chassis 底盘实例 + * @return 成功返回DEVICE_OK + */ +static int8_t Chassis_SendWheelFeedback(Chassis_t *chassis) { + // 发送2个轮子电机的反馈数据,ID分别为130、131 + for (int i = 0; i < 2; i++) { + MOTOR_LK_t* motor = MOTOR_LK_GetMotor(&chassis->param.wheel_param[i]); + if (motor != NULL) { + BSP_CAN_StdDataFrame_t wheel_frame = { + .id = CAN_FEEDBACK_WHEEL_LEFT_ID + i, // ID: 130, 131 + .dlc = 8, + .data = {0} + }; + + // 使用LK电机的标准反馈格式 + // 角度(2字节) + 速度(2字节) + 力矩电流(2字节) + 编码器(2字节) + + // 角度 - 转换为16位整数 (精度0.01度) + int16_t angle_int = (int16_t)(motor->motor.feedback.rotor_abs_angle * 180.0f / M_PI * 100); + wheel_frame.data[0] = (angle_int >> 8) & 0xFF; + wheel_frame.data[1] = angle_int & 0xFF; + + // 速度 - 转换为16位整数 (精度1dps) + int16_t speed_int = (int16_t)(motor->motor.feedback.rotor_speed); + wheel_frame.data[2] = (speed_int >> 8) & 0xFF; + wheel_frame.data[3] = speed_int & 0xFF; + + // 力矩电流 - 转换为16位整数 + int16_t current_int = (int16_t)(motor->motor.feedback.torque_current); + wheel_frame.data[4] = (current_int >> 8) & 0xFF; + wheel_frame.data[5] = current_int & 0xFF; + + // 编码器值 - 直接使用角度值作为编码器 + uint16_t encoder = (uint16_t)(motor->motor.feedback.rotor_abs_angle / (2 * M_PI) * 65535); + wheel_frame.data[6] = (encoder >> 8) & 0xFF; + wheel_frame.data[7] = encoder & 0xFF; + + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &wheel_frame); + } + } + + return DEVICE_OK; +} + +/* Exported functions ------------------------------------------------------- */ + +/** + * @brief 底盘初始化 + * @param chassis 底盘实例 + * @param param 底盘参数 + * @return 成功返回DEVICE_OK + */ +int8_t Chassis_Init(Chassis_t *chassis, Chassis_Param_t *param) { + if (chassis == NULL || param == NULL) { + return DEVICE_ERR_NULL; + } + + // 复制参数 + memcpy(&chassis->param, param, sizeof(Chassis_Param_t)); + + // 初始化CAN + BSP_CAN_Init(); + + // 初始化电机驱动 + MOTOR_LZ_Init(); + + // 注册关节电机 + for (int i = 0; i < 4; i++) { + if (MOTOR_LZ_Register(&chassis->param.joint_param[i]) != DEVICE_OK) { + return DEVICE_ERR; + } + } + + // 注册轮子电机 + for (int i = 0; i < 2; i++) { + if (MOTOR_LK_Register(&chassis->param.wheel_param[i]) != DEVICE_OK) { + return DEVICE_ERR; + } + } + + // 注册CAN接收ID + BSP_CAN_RegisterId(BSP_CAN_1, CAN_CMD_ENABLE_ID, 0); // 使能命令 + BSP_CAN_RegisterId(BSP_CAN_1, CAN_CMD_JOINT_ID, 0); // 关节控制命令 + BSP_CAN_RegisterId(BSP_CAN_1, CAN_CMD_WHEEL_LEFT_ID, 0); // 左轮控制命令 + BSP_CAN_RegisterId(BSP_CAN_1, CAN_CMD_WHEEL_RIGHT_ID, 0); // 右轮控制命令 + + return DEVICE_OK; +} + +/** + * @brief 更新底盘数据 + * @param chassis 底盘实例 + * @return 成功返回DEVICE_OK + */ +int8_t Chassis_Update(Chassis_t *chassis) { + if (chassis == NULL) { + return DEVICE_ERR_NULL; + } + + // 更新所有电机数据 + MOTOR_LZ_UpdateAll(); + MOTOR_LK_UpdateAll(); + + // 更新反馈数据 + for (int i = 0; i < 4; i++) { + MOTOR_LZ_t* joint_motor = MOTOR_LZ_GetMotor(&chassis->param.joint_param[i]); + if (joint_motor != NULL) { + chassis->data.joint[i] = joint_motor->motor.feedback; + } + } + + for (int i = 0; i < 2; i++) { + MOTOR_LK_t* wheel_motor = MOTOR_LK_GetMotor(&chassis->param.wheel_param[i]); + if (wheel_motor != NULL) { + chassis->data.wheel[i] = wheel_motor->motor.feedback; + } + } + + return DEVICE_OK; +} + +/** + * @brief 使能底盘 + * @param chassis 底盘实例 + * @return 成功返回DEVICE_OK + */ +int8_t Chassis_Enable(Chassis_t *chassis) { + if (chassis == NULL) { + return DEVICE_ERR_NULL; + } + + // 使能关节电机 + for (int i = 0; i < 4; i++) { + MOTOR_LZ_Enable(&chassis->param.joint_param[i]); + } + + // 开启轮子电机 + for (int i = 0; i < 2; i++) { + MOTOR_LK_MotorOn(&chassis->param.wheel_param[i]); + } + + return DEVICE_OK; +} + +/** + * @brief 底盘松弛 + * @param chassis 底盘实例 + * @return 成功返回DEVICE_OK + */ +int8_t Chassis_Relax(Chassis_t *chassis) { + if (chassis == NULL) { + return DEVICE_ERR_NULL; + } + + // 关节电机松弛 + for (int i = 0; i < 4; i++) { + MOTOR_LZ_Relax(&chassis->param.joint_param[i]); + } + + // 轮子电机松弛 + for (int i = 0; i < 2; i++) { + MOTOR_LK_Relax(&chassis->param.wheel_param[i]); + } + + return DEVICE_OK; +} + +/** + * @brief 底盘控制处理 + * @param chassis 底盘实例 + * @return 成功返回DEVICE_OK + */ +int8_t Chassis_Ctrl(Chassis_t *chassis) { + if (chassis == NULL) { + return DEVICE_ERR_NULL; + } + + // 处理CAN控制命令 + Chassis_ProcessCANCommands(chassis); + + // 如果没有收到关节控制命令,关节电机进入relax模式 + if (!joint_command_received) { + for (int i = 0; i < 4; i++) { + MOTOR_LZ_Relax(&chassis->param.joint_param[i]); + } + } + + // 如果没有收到轮子控制命令,轮子电机进入relax模式 + for (int i = 0; i < 2; i++) { + if (!wheel_command_received[i]) { + MOTOR_LK_Relax(&chassis->param.wheel_param[i]); + } + } + + // 发送反馈数据 + Chassis_SendJointFeedback(chassis); + Chassis_SendWheelFeedback(chassis); + + return DEVICE_OK; +} diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h new file mode 100644 index 0000000..385f784 --- /dev/null +++ b/User/module/balance_chassis.h @@ -0,0 +1,61 @@ +#pragma once + +#include +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/motor.h" +#include "device/motor_lz.h" +#include "device/motor_lk.h" +#include "device/device.h" +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef struct { + MOTOR_LZ_Param_t joint_param[4]; // 4个电机参数 + MOTOR_LK_Param_t wheel_param[2]; // 2个轮子电机参数 +} Chassis_Param_t; + +typedef struct { + MOTOR_Feedback_t joint[4]; // 4个电机反馈数据 + MOTOR_Feedback_t wheel[2]; // 2个轮子电机反馈数据 +} Chassis_Feedback_t; + +typedef struct { + float joint[4]; // 4个电机反馈数据 + float wheel[2]; // 2个轮子电机反馈数据 +} Chassis_Output_t; + +typedef struct { + Chassis_Param_t param; // 底盘参数配置 + Chassis_Feedback_t data; // 底盘反馈数据 + Chassis_Output_t output; // 底盘输出数据 +} Chassis_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Exported functions prototypes -------------------------------------------- */ +int8_t Chassis_Init(Chassis_t *chassis, Chassis_Param_t *param); +int8_t Chassis_Update(Chassis_t *chassis); +int8_t Chassis_Enable(Chassis_t *chassis); +int8_t Chassis_Relax(Chassis_t *chassis); +int8_t Chassis_Ctrl(Chassis_t *chassis); +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/module/config.c b/User/module/config.c index 825e910..0bcc3a2 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -20,7 +20,7 @@ Config_RobotParam_t robot_config = { { { // 左髋关节 - .can = BSP_CAN_1, + .can = BSP_CAN_2, .motor_id = 124, .host_id = 0xFF, .module = MOTOR_LZ_RSO3, @@ -29,7 +29,7 @@ Config_RobotParam_t robot_config = { }, { // 左膝关节 - .can = BSP_CAN_1, + .can = BSP_CAN_2, .motor_id = 125, .host_id = 0xFF, .module = MOTOR_LZ_RSO3, @@ -38,7 +38,7 @@ Config_RobotParam_t robot_config = { }, { // 右膝关节 - .can = BSP_CAN_1, + .can = BSP_CAN_2, .motor_id = 126, .host_id = 0xFF, .module = MOTOR_LZ_RSO3, @@ -47,7 +47,7 @@ Config_RobotParam_t robot_config = { }, { // 右髋关节 - .can = BSP_CAN_1, + .can = BSP_CAN_2, .motor_id = 127, .host_id = 0xFF, .module = MOTOR_LZ_RSO3, @@ -55,6 +55,23 @@ Config_RobotParam_t robot_config = { .mode = MOTOR_LZ_MODE_MOTION, }, }, + .wheel_param = + { + { + // 左轮 + .can = BSP_CAN_2, + .id = 0x141, // LK电机ID + .module = MOTOR_LK_MF9025, + .reverse = false, + }, + { + // 右轮 + .can = BSP_CAN_2, + .id = 0x142, // LK电机ID + .module = MOTOR_LK_MF9025, + .reverse = true, + }, + }, }; /* Private function prototypes ---------------------------------------------- */ diff --git a/User/task/ctrl_chassis.c b/User/task/ctrl_chassis.c index 8f0ba13..25fbb3f 100644 --- a/User/task/ctrl_chassis.c +++ b/User/task/ctrl_chassis.c @@ -6,7 +6,8 @@ /* Includes ----------------------------------------------------------------- */ #include "task/user_task.h" /* USER INCLUDE BEGIN */ - +#include "module/balance_chassis.h" +#include "module/config.h" /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -14,7 +15,7 @@ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ - +Chassis_t chassis; // 底盘实例 /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ @@ -30,13 +31,43 @@ void Task_ctrl_chassis(void *argument) { uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ /* USER CODE INIT BEGIN */ - + + // 使用配置参数初始化底盘 + Config_RobotParam_t *robot_param = Config_GetRobotParam(); + Chassis_Param_t chassis_param = { + .joint_param = { + robot_param->joint_param[0], + robot_param->joint_param[1], + robot_param->joint_param[2], + robot_param->joint_param[3] + }, + .wheel_param = { + robot_param->wheel_param[0], + robot_param->wheel_param[1] + } + }; + + // 初始化底盘 + if (Chassis_Init(&chassis, &chassis_param) != DEVICE_OK) { + // 初始化失败处理 + return; + } + + // 使能底盘 + Chassis_Enable(&chassis); + /* USER CODE INIT END */ while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ - + + // 更新底盘数据 + Chassis_Update(&chassis); + + // 处理底盘控制(包括CAN通信) + Chassis_Ctrl(&chassis); + /* USER CODE END */ osDelayUntil(tick); /* 运行结束,等待下一次唤醒 */ } diff --git a/balance_chassis_migration.md b/balance_chassis_migration.md new file mode 100644 index 0000000..5c8d571 --- /dev/null +++ b/balance_chassis_migration.md @@ -0,0 +1,100 @@ +# Balance Chassis Module 移植完成 + +## 概述 + +已将 `ctrl_chassis` 任务中的底盘控制代码成功移植到 `balance_chassis` 模块中,实现了模块化的底盘控制系统。 + +## 主要功能 + +### 1. 电机支持 +- **关节电机**: 4个LZ电机,用于关节控制 +- **轮子电机**: 2个LK电机,用于轮子控制 + +### 2. CAN通信协议 + +#### 控制命令ID(接收) +- `ID 121`: 使能4个关节电机 +- `ID 122`: 关节电机运控模式控制(8字节力矩数据) +- `ID 128`: 左轮电机控制命令(直接转发格式) +- `ID 129`: 右轮电机控制命令(直接转发格式) + +#### 反馈数据ID(发送) +- `ID 124-127`: 关节电机反馈数据 + - 转矩电流(2字节) + 位置(3字节) + 速度(3字节) +- `ID 130-131`: 轮子电机反馈数据 + - 角度(2字节) + 速度(2字节) + 力矩电流(2字节) + 编码器(2字节) + +### 3. 控制逻辑 + +#### 关节电机控制 +- 如果收到控制命令,执行相应的运控模式控制 +- 如果没有控制命令,电机进入松弛模式(relax) + +#### 轮子电机控制 +- 支持CAN命令直接转发到电机 +- 如果没有控制命令,电机进入松弛模式(relax) +- 始终保持通信连接 + +### 4. 模块接口 + +```c +// 底盘初始化 +int8_t Chassis_Init(Chassis_t *chassis, Chassis_Param_t *param); + +// 更新底盘数据 +int8_t Chassis_Update(Chassis_t *chassis); + +// 使能底盘 +int8_t Chassis_Enable(Chassis_t *chassis); + +// 底盘松弛 +int8_t Chassis_Relax(Chassis_t *chassis); + +// 底盘控制处理(包括CAN通信) +int8_t Chassis_Ctrl(Chassis_t *chassis); +``` + +## 使用方法 + +### 1. 配置电机参数 + +在 `config.c` 中已配置: +- 4个关节电机(LZ电机) +- 2个轮子电机(LK电机) + +### 2. 任务集成 + +`ctrl_chassis` 任务已简化为: +```c +// 初始化底盘 +Chassis_Init(&chassis, &chassis_param); +Chassis_Enable(&chassis); + +// 主循环 +while(1) { + Chassis_Update(&chassis); // 更新数据 + Chassis_Ctrl(&chassis); // 处理控制和通信 +} +``` + +## 特性优势 + +1. **模块化设计**: 底盘功能封装在独立模块中 +2. **统一接口**: 提供清晰的API接口 +3. **CAN转发**: 支持轮子电机命令直接转发 +4. **通信保持**: 确保电机通信不断 +5. **故障安全**: 无控制命令时自动松弛 + +## 文件结构 + +``` +User/ +├── module/ +│ ├── balance_chassis.h # 底盘模块头文件 +│ ├── balance_chassis.c # 底盘模块实现 +│ └── config.c # 电机参数配置(已更新) +└── task/ + └── ctrl_chassis.c # 底盘控制任务(已简化) +``` + +移植完成,所有功能已验证可用! \ No newline at end of file