diff --git a/User/device/virtual_chassis.c b/User/device/virtual_chassis.c new file mode 100644 index 0000000..ef9db68 --- /dev/null +++ b/User/device/virtual_chassis.c @@ -0,0 +1,417 @@ +/* + * @file virtual_chassis.c + * @brief 虚拟底盘设备驱动实现 - 控制端 + * @details 作为控制端，发送控制命令到底盘，接收底盘反馈数据 + */ + +/* Includes ----------------------------------------------------------------- */ +#include "device/virtual_chassis.h" +#include "bsp/can.h" +#include "bsp/time.h" +#include + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ + + +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* Private function prototypes ---------------------------------------------- */ +static int8_t Virtual_Chassis_DecodeMotor(Virtual_Chassis_t *chassis, uint8_t id, uint8_t *data) { + if (chassis == NULL || data == NULL) { + return DEVICE_ERR_NULL; + } + + // 1. 解析转矩电流 - 前2字节，16位有符号整数 (精度0.01 Nm) + int16_t torque_int; + memcpy(&torque_int, &data[0], sizeof(int16_t)); + chassis->data.joint[id-chassis->param.motors.feedback_id].torque_current = (float)torque_int / 100.0f; + + // 2. 解析位置 - 第3-5字节，24位有符号整数 (精度0.0001 rad) + int32_t angle_int = 0; + angle_int |= ((int32_t)data[2]) << 16; + angle_int |= ((int32_t)data[3]) << 8; + angle_int |= ((int32_t)data[4]); + + // 符号扩展（24位转32位） + if (angle_int & 0x800000) { + angle_int |= 0xFF000000; + } + chassis->data.joint[id-chassis->param.motors.feedback_id].rotor_abs_angle = (float)angle_int / 10000.0f; + + // 3. 解析速度 - 第6-8字节，24位有符号整数 (精度0.001 rad/s) + int32_t velocity_int = 0; + velocity_int |= ((int32_t)data[5]) << 16; + velocity_int |= ((int32_t)data[6]) << 8; + velocity_int |= ((int32_t)data[7]); + + // 符号扩展（24位转32位） + if (velocity_int & 0x800000) { + velocity_int |= 0xFF000000; + } + chassis->data.joint[id-chassis->param.motors.feedback_id].rotor_speed = (float)velocity_int / 1000.0f; + + chassis->data.joint[id-chassis->param.motors.feedback_id].temp = 0.0f; + + return DEVICE_OK; +} + +static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint8_t id, uint8_t *data, uint8_t len) { + if (chassis == NULL || data == NULL || len < 8) { + return DEVICE_ERR_NULL; + } + + float *float_data = (float*)data; + + switch (id) { + case 0x66: // 加速度计数据 - 8字节，2个float值 (x, y) + chassis->data.imu.accl.x = float_data[0]; + chassis->data.imu.accl.y = float_data[1]; + // z轴数据未传输，保持之前的值或设为0 + // chassis->data.imu.accl.z = 0.0f; + break; + + case 0x67: // 陀螺仪数据 - 8字节，2个float值 (x, y) + chassis->data.imu.gyro.x = float_data[0]; + chassis->data.imu.gyro.y = float_data[1]; + // z轴数据未传输，保持之前的值或设为0 + // chassis->data.imu.gyro.z = 0.0f; + break; + + case 0x68: // 欧拉角数据 - 8字节，2个float值 (yaw, pitch) + chassis->data.imu.euler.yaw = float_data[0]; + chassis->data.imu.euler.pit = float_data[1]; + // roll轴数据未传输，保持之前的值或设为0 + // chassis->data.imu.euler.rol = 0.0f; + break; + + case 0x69: // 四元数数据 - 8字节，2个float值 (q0, q1) + chassis->data.imu.quat.q0 = float_data[0]; + chassis->data.imu.quat.q1 = float_data[1]; + // q2, q3数据未传输，保持之前的值或设为0 + // chassis->data.imu.quat.q2 = 0.0f; + // chassis->data.imu.quat.q3 = 0.0f; + break; + + default: + return DEVICE_ERR; + } + + return DEVICE_OK; +} + +/* Exported functions ------------------------------------------------------- */ + +/** + * @brief 初始化虚拟底盘设备 + */ +int8_t Virtual_Chassis_Init(Virtual_Chassis_t *chassis, Virtual_Chassis_Param_t *param) { + if (chassis == NULL || param == NULL) { + return DEVICE_ERR_NULL; + } + + // 复制参数配置 + chassis->param = *param; + + // 初始化设备头部 + chassis->header.online = false; + chassis->header.last_online_time = 0; + + // 初始化数据 + memset(&chassis->data, 0, sizeof(Virtual_Chassis_Feedback_t)); + memset(&chassis->output, 0, sizeof(Virtual_Chassis_Output_t)); + + BSP_CAN_Init(); + + for (int i = 0; i < 4; i++) { + BSP_CAN_RegisterId(chassis->param.motors.can, 124 + i, 3); // 电机反馈ID + BSP_CAN_RegisterId(chassis->param.imu.can, 0x66 + 1, 3); + } + + + // 设置设备在线状态 + chassis->header.online = true; + chassis->header.last_online_time = BSP_TIME_Get_us(); + + return DEVICE_OK; +} + + +/** + * @brief 更新虚拟底盘数据（接收底盘反馈数据） + */ +int8_t Virtual_Chassis_Update(Virtual_Chassis_t *chassis) { + if (chassis == NULL) { + return DEVICE_ERR_NULL; + } + + BSP_CAN_Message_t msg; + BSP_CAN_GetMessage(chassis->param.motors.can, 124, &msg, BSP_CAN_TIMEOUT_IMMEDIATE); + + + return DEVICE_OK; +} + +/** + * @brief 接收底盘反馈数据 + */ +int8_t Virtual_Chassis_ReceiveFeedback(Virtual_Chassis_t *chassis) { + if (chassis == NULL) { + return DEVICE_ERR_NULL; + } + + // 处理4个电机的反馈数据 (ID 124-127) + for (int i = 0; i < VIRTUAL_CHASSIS_MOTOR_COUNT; i++) { + Virtual_Chassis_ProcessMotorFeedback(chassis, i); + } + + // 处理IMU反馈数据 + Virtual_Chassis_ProcessIMUFeedback(chassis); + + return DEVICE_OK; +} + +/** + * @brief 发送电机使能命令 + */ +int8_t Virtual_Chassis_Enable(Virtual_Chassis_t *chassis) { + if (chassis == NULL) { + return DEVICE_ERR_NULL; + } + + BSP_CAN_StdDataFrame_t enable_frame = { + .id = chassis->param.motors.Enable_id, + }; + + if (BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &enable_frame) == BSP_OK) { + return DEVICE_OK; + } + + return DEVICE_ERR; +} + +/** + * @brief 发送电机力矩控制命令 + */ +int8_t Virtual_Chassis_SendTorqueCommand(Virtual_Chassis_t *chassis, float torques[4]) { + if (chassis == NULL || torques == NULL) { + return DEVICE_ERR_NULL; + } + + BSP_CAN_StdDataFrame_t torque_frame = { + .id = chassis->param.motors.MIT_id, + .dlc = 8, + .data = {0} + }; + + // 8字节数据分别是4个电机的力矩 (每个电机2字节，有符号整数，精度0.01 Nm) + for (int i = 0; i < 4; i++) { + int16_t torque_raw = (int16_t)(torques[i] * 100.0f); + memcpy(&torque_frame.data[i * 2], &torque_raw, sizeof(int16_t)); + chassis->output.joint_torque[i] = torques[i]; + } + + return BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &torque_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +/** + * @brief 获取电机反馈数据 + */ +Virtual_Chassis_MotorFeedback_t* Virtual_Chassis_GetMotorFeedback(Virtual_Chassis_t *chassis, uint8_t motor_id) { + if (chassis == NULL || motor_id >= VIRTUAL_CHASSIS_MOTOR_COUNT) { + return NULL; + } + + return &chassis->data.motors[motor_id]; +} + +/** + * @brief 获取IMU反馈数据 + */ +Virtual_Chassis_IMUFeedback_t* Virtual_Chassis_GetIMUFeedback(Virtual_Chassis_t *chassis) { + if (chassis == NULL) { + return NULL; + } + + return &chassis->data.imu; +} + +/** + * @brief 设置目标力矩 + */ +int8_t Virtual_Chassis_SetTargetTorques(Virtual_Chassis_t *chassis, float torques[VIRTUAL_CHASSIS_MOTOR_COUNT]) { + if (chassis == NULL || torques == NULL) { + return DEVICE_ERR_NULL; + } + + memcpy(chassis->data.target_torques, torques, sizeof(float) * VIRTUAL_CHASSIS_MOTOR_COUNT); + return DEVICE_OK; +} + +/** + * @brief 使能电机并发送命令 + */ +int8_t Virtual_Chassis_Enable(Virtual_Chassis_t *chassis) { + if (chassis == NULL) { + return DEVICE_ERR_NULL; + } + + return Virtual_Chassis_SendEnableCommand(chassis); +} + +/** + * @brief 检查虚拟底盘是否在线 + */ +bool Virtual_Chassis_IsOnline(Virtual_Chassis_t *chassis) { + if (chassis == NULL) { + return false; + } + + uint64_t current_time = BSP_TIME_Get_us(); + uint64_t timeout_us = VIRTUAL_CHASSIS_TIMEOUT_MS * 1000; + + return chassis->header.online && + (current_time - chassis->header.last_online_time) < timeout_us; +} + +/* Private functions -------------------------------------------------------- */ + +/** + * @brief 注册CAN接收ID (接收底盘反馈数据) + */ +static int8_t Virtual_Chassis_RegisterCANIds(Virtual_Chassis_t *chassis) { + if (chassis == NULL) { + return DEVICE_ERR_NULL; + } + + // 注册电机反馈数据ID + for (int i = 0; i < VIRTUAL_CHASSIS_MOTOR_COUNT; i++) { + if (BSP_CAN_RegisterId(chassis->param.motors[i].can, chassis->param.motors[i].feedback_id, 0) != BSP_OK) { + return DEVICE_ERR; + } + } + + // 注册IMU反馈数据ID + if (BSP_CAN_RegisterId(chassis->param.imu_can, chassis->param.accl_id, 0) != BSP_OK) { + return DEVICE_ERR; + } + + if (BSP_CAN_RegisterId(chassis->param.imu_can, chassis->param.gyro_id, 0) != BSP_OK) { + return DEVICE_ERR; + } + + if (BSP_CAN_RegisterId(chassis->param.imu_can, chassis->param.euler_id, 0) != BSP_OK) { + return DEVICE_ERR; + } + + if (BSP_CAN_RegisterId(chassis->param.imu_can, chassis->param.quat_id, 0) != BSP_OK) { + return DEVICE_ERR; + } + + return DEVICE_OK; +} + +/** + * @brief 处理电机反馈数据 + */ +static int8_t Virtual_Chassis_ProcessMotorFeedback(Virtual_Chassis_t *chassis, uint8_t motor_id) { + BSP_CAN_Message_t rx_msg; + + if (BSP_CAN_GetMessage(chassis->param.motors[motor_id].can, chassis->param.motors[motor_id].feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + // 解析数据：转矩电流(2字节) + 位置(3字节) + 速度(3字节) = 8字节 + + // 转矩电流 (精度0.01 Nm) + int16_t torque_int; + memcpy(&torque_int, &rx_msg.data[0], sizeof(int16_t)); + chassis->data.motors[motor_id].current_torque = (float)torque_int / 100.0f; + + // 位置 (精度0.0001 rad) + int32_t angle_int = 0; + angle_int |= ((int32_t)rx_msg.data[2]) << 16; + angle_int |= ((int32_t)rx_msg.data[3]) << 8; + angle_int |= ((int32_t)rx_msg.data[4]); + if (angle_int & 0x800000) { // 符号扩展 + angle_int |= 0xFF000000; + } + chassis->data.motors[motor_id].current_angle = (float)angle_int / 10000.0f; + + // 速度 (精度0.001 rad/s) + int32_t velocity_int = 0; + velocity_int |= ((int32_t)rx_msg.data[5]) << 16; + velocity_int |= ((int32_t)rx_msg.data[6]) << 8; + velocity_int |= ((int32_t)rx_msg.data[7]); + if (velocity_int & 0x800000) { // 符号扩展 + velocity_int |= 0xFF000000; + } + chassis->data.motors[motor_id].current_velocity = (float)velocity_int / 1000.0f; + + // 更新在线状态 + chassis->data.motors[motor_id].online = true; + chassis->data.motors[motor_id].last_update_time = BSP_TIME_Get_us(); + + return DEVICE_OK; + } + + return DEVICE_ERR; +} + +/** + * @brief 处理IMU反馈数据 + */ +static int8_t Virtual_Chassis_ProcessIMUFeedback(Virtual_Chassis_t *chassis) { + BSP_CAN_Message_t rx_msg; + bool imu_updated = false; + + // 处理加速度计数据 + if (BSP_CAN_GetMessage(chassis->param.imu_can, chassis->param.accl_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + float *accl_data = (float*)rx_msg.data; + chassis->data.imu.accl.x = accl_data[0]; + chassis->data.imu.accl.y = accl_data[1]; + if (rx_msg.dlc >= 12) { // 如果有第三个数据 + chassis->data.imu.accl.z = accl_data[2]; + } + imu_updated = true; + } + + // 处理陀螺仪数据 + if (BSP_CAN_GetMessage(chassis->param.imu_can, chassis->param.gyro_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + float *gyro_data = (float*)rx_msg.data; + chassis->data.imu.gyro.x = gyro_data[0]; + chassis->data.imu.gyro.y = gyro_data[1]; + if (rx_msg.dlc >= 12) { // 如果有第三个数据 + chassis->data.imu.gyro.z = gyro_data[2]; + } + imu_updated = true; + } + + // 处理欧拉角数据 + if (BSP_CAN_GetMessage(chassis->param.imu_can, chassis->param.euler_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + float *euler_data = (float*)rx_msg.data; + chassis->data.imu.euler.yaw = euler_data[0]; + chassis->data.imu.euler.pit = euler_data[1]; + if (rx_msg.dlc >= 12) { // 如果有第三个数据 + chassis->data.imu.euler.rol = euler_data[2]; + } + imu_updated = true; + } + + // 处理四元数数据（可选） + if (BSP_CAN_GetMessage(chassis->param.imu_can, chassis->param.quat_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + float *quat_data = (float*)rx_msg.data; + chassis->data.imu.quat.q0 = quat_data[0]; + chassis->data.imu.quat.q1 = quat_data[1]; + if (rx_msg.dlc >= 16) { // 如果有更多数据 + chassis->data.imu.quat.q2 = quat_data[2]; + chassis->data.imu.quat.q3 = quat_data[3]; + } + imu_updated = true; + } + + if (imu_updated) { + chassis->data.imu.online = true; + chassis->data.imu.last_update_time = BSP_TIME_Get_us(); + return DEVICE_OK; + } + + return DEVICE_ERR; +} \ No newline at end of file diff --git a/User/device/virtual_chassis.h b/User/device/virtual_chassis.h new file mode 100644 index 0000000..6195366 --- /dev/null +++ b/User/device/virtual_chassis.h @@ -0,0 +1,151 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" +#include "device/motor.h" +#include "component/ahrs.h" +#include "bsp/can.h" + +/* Exported constants ------------------------------------------------------- */ + + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ + +/* 电机反馈数据结构体 */ +typedef struct { + MOTOR_Feedback_t joint[4]; // 4个电机反馈数据 + struct { + DEVICE_Header_t header; // 设备通用头部 + AHRS_Accl_t accl; // 加速度计数据 + AHRS_Gyro_t gyro; // 陀螺仪数据 + AHRS_Eulr_t euler; // 欧拉角数据 + AHRS_Quaternion_t quat; // 四元数数据 + } imu; +} Virtual_Chassis_Feedback_t; + +typedef struct { + float joint_torque[4]; // 4个电机目标力矩 +} Virtual_Chassis_Output_t; + +/* 电机CAN参数结构体 */ +typedef struct { + BSP_CAN_t can; // 电机所在CAN总线 + uint16_t Enable_id; // 电机使能命令CAN ID + uint16_t MIT_id; // 电机力矩控制命令CAN ID + uint16_t feedback_id; // 电机反馈数据CAN ID +} Virtual_Chassis_MotorParam_t; + +/* IMU参数结构体 */ +typedef struct { + BSP_CAN_t can; // 电机所在CAN总线 + uint16_t accl_id; // 加速度计数据CAN ID + uint16_t gyro_id; // 陀螺仪数据CAN ID + uint16_t euler_id; // 欧拉角数据CAN ID + uint16_t quat_id; // 四元数数据CAN ID +} Virtual_Chassis_IMUParam_t; + +/* 虚拟底盘参数配置结构体 */ +typedef struct { + Virtual_Chassis_MotorParam_t motors; // 4个电机CAN参数 + Virtual_Chassis_IMUParam_t imu; // IMU CAN参数 +} Virtual_Chassis_Param_t; + +/* 虚拟底盘设备结构体 */ +typedef struct { + DEVICE_Header_t header; // 设备通用头部 + Virtual_Chassis_Param_t param; // 参数配置 + Virtual_Chassis_Feedback_t data; // 反馈数据 + Virtual_Chassis_Output_t output; // 控制输出 +} Virtual_Chassis_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 初始化虚拟底盘设备 + * @param chassis 虚拟底盘设备结构体指针 + * @param param 虚拟底盘参数配置指针 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t Virtual_Chassis_Init(Virtual_Chassis_t *chassis, Virtual_Chassis_Param_t *param); + +/** + * @brief 反初始化虚拟底盘设备 + * @param chassis 虚拟底盘设备结构体指针 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t Virtual_Chassis_DeInit(Virtual_Chassis_t *chassis); + +/** + * @brief 更新虚拟底盘数据（包括电机和IMU数据） + * @param chassis 虚拟底盘设备结构体指针 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t Virtual_Chassis_Update(Virtual_Chassis_t *chassis); + +/** + * @brief 接收底盘反馈数据 + * @param chassis 虚拟底盘设备结构体指针 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t Virtual_Chassis_ReceiveFeedback(Virtual_Chassis_t *chassis); + +/** + * @brief 发送电机使能命令 + * @param chassis 虚拟底盘设备结构体指针 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t Virtual_Chassis_SendEnableCommand(Virtual_Chassis_t *chassis); + +/** + * @brief 发送电机力矩控制命令 + * @param chassis 虚拟底盘设备结构体指针 + * @param torques 4个电机的目标力矩数组 (-60~60 Nm) + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t Virtual_Chassis_SendTorqueCommand(Virtual_Chassis_t *chassis, float torques[VIRTUAL_CHASSIS_MOTOR_COUNT]); + +/** + * @brief 获取电机反馈数据 + * @param chassis 虚拟底盘设备结构体指针 + * @param motor_id 电机ID (0-3) + * @return 电机反馈数据指针，失败返回NULL + */ +Virtual_Chassis_MotorFeedback_t* Virtual_Chassis_GetMotorFeedback(Virtual_Chassis_t *chassis, uint8_t motor_id); + +/** + * @brief 获取IMU反馈数据 + * @param chassis 虚拟底盘设备结构体指针 + * @return IMU反馈数据指针，失败返回NULL + */ +Virtual_Chassis_IMUFeedback_t* Virtual_Chassis_GetIMUFeedback(Virtual_Chassis_t *chassis); + +/** + * @brief 设置目标力矩 + * @param chassis 虚拟底盘设备结构体指针 + * @param torques 4个电机的目标力矩数组 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t Virtual_Chassis_SetTargetTorques(Virtual_Chassis_t *chassis, float torques[VIRTUAL_CHASSIS_MOTOR_COUNT]); + +/** + * @brief 使能电机并发送命令 + * @param chassis 虚拟底盘设备结构体指针 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t Virtual_Chassis_Enable(Virtual_Chassis_t *chassis); + +/** + * @brief 检查虚拟底盘是否在线 + * @param chassis 虚拟底盘设备结构体指针 + * @return true 在线，false 离线 + */ +bool Virtual_Chassis_IsOnline(Virtual_Chassis_t *chassis); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index f353ee5..7f7c729 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -19,7 +19,7 @@ static int8_t Chassis_MotorEnable(Chassis_t *c) { if (c == NULL) return CHASSIS_ERR_NULL; for (int i = 0; i < 4; i++) { - MOTOR_LZ_Enable(&c->param->joint_motors[i]); + // MOTOR_LZ_Enable(&c->param->joint_motors[i]); } BSP_TIME_Delay_us(200); for (int i = 0; i < 2; i++) { @@ -32,7 +32,7 @@ static int8_t Chassis_MotorRelax(Chassis_t *c) { if (c == NULL) return CHASSIS_ERR_NULL; for (int i = 0; i < 2; i++) { - MOTOR_LK_Relax(&c->param->wheel_motors[i]); + // MOTOR_LK_Relax(&c->param->wheel_motors[i]); } BSP_TIME_Delay_us(200); for (int i = 0; i < 4; i++) { @@ -275,7 +275,7 @@ void Chassis_Output(Chassis_t *c) { for (int i = 0; i < 4; i++) { MOTOR_LZ_MotionParam_t param = {0}; param.torque = c->output.joint[i].torque; - MOTOR_LZ_MotionControl(&c->param->joint_motors[i], ¶m); + // MOTOR_LZ_MotionControl(&c->param->joint_motors[i], ¶m); } BSP_TIME_Delay_us(400); // 等待CAN总线空闲，确保前面的命令发送完成 for (int i = 0; i < 2; i++) { diff --git a/User/module/config.c b/User/module/config.c index d330e91..7789fa1 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -183,6 +183,37 @@ Config_RobotParam_t robot_config = { .mouse_id = 0x32, .keyboard_id = 0x33, }, + + .virtual_chassis_param = { + .control_can = BSP_CAN_1, + .enable_id = VIRTUAL_CHASSIS_CAN_ID_ENABLE_DEFAULT, + .control_id = VIRTUAL_CHASSIS_CAN_ID_CONTROL_DEFAULT, + + .motors = { + { // 电机0 - 左髋关节反馈 + .can = BSP_CAN_1, + .feedback_id = VIRTUAL_CHASSIS_CAN_ID_FEEDBACK_BASE_DEFAULT + 0, // 124 + }, + { // 电机1 - 左膝关节反馈 + .can = BSP_CAN_1, + .feedback_id = VIRTUAL_CHASSIS_CAN_ID_FEEDBACK_BASE_DEFAULT + 1, // 125 + }, + { // 电机2 - 右膝关节反馈 + .can = BSP_CAN_1, + .feedback_id = VIRTUAL_CHASSIS_CAN_ID_FEEDBACK_BASE_DEFAULT + 2, // 126 + }, + { // 电机3 - 右髋关节反馈 + .can = BSP_CAN_1, + .feedback_id = VIRTUAL_CHASSIS_CAN_ID_FEEDBACK_BASE_DEFAULT + 3, // 127 + }, + }, + + .imu_can = BSP_CAN_1, + .accl_id = VIRTUAL_CHASSIS_CAN_ID_ACCL_DEFAULT, + .gyro_id = VIRTUAL_CHASSIS_CAN_ID_GYRO_DEFAULT, + .euler_id = VIRTUAL_CHASSIS_CAN_ID_EULER_DEFAULT, + .quat_id = VIRTUAL_CHASSIS_CAN_ID_QUAT_DEFAULT, + }, }; /* Private function prototypes ---------------------------------------------- */ diff --git a/User/module/config.h b/User/module/config.h index 712bcbc..ef1adc5 100644 --- a/User/module/config.h +++ b/User/module/config.h @@ -14,11 +14,13 @@ extern "C" { #include "device/motor_lk.h" #include "module/balance_chassis.h" #include "device/rc_can.h" +#include "device/virtual_chassis.h" typedef struct { DM_IMU_Param_t imu_param; Chassis_Params_t chassis_param; RC_CAN_Param_t rc_can_param; + Virtual_Chassis_Param_t virtual_chassis_param; // 虚拟底盘参数 } Config_RobotParam_t; /* Exported functions prototypes -------------------------------------------- */ diff --git a/User/task/atti_esti.c b/User/task/atti_esti.c index 4c666b3..0ef10e8 100644 --- a/User/task/atti_esti.c +++ b/User/task/atti_esti.c @@ -43,13 +43,13 @@ void Task_atti_esti(void *argument) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ - if (DM_IMU_AutoUpdateAll(&dm_imu) == DEVICE_OK) { - osMessageQueueReset(task_runtime.msgq.chassis_imu); // 重置消息队列，防止阻塞 - chassis_imu_send.accl = dm_imu.data.accl; - chassis_imu_send.gyro = dm_imu.data.gyro; - chassis_imu_send.euler = dm_imu.data.euler; - osMessageQueuePut(task_runtime.msgq.chassis_imu, &chassis_imu_send, 0, 0); // 非阻塞发送IMU数据 - } + // if (DM_IMU_AutoUpdateAll(&dm_imu) == DEVICE_OK) { + // osMessageQueueReset(task_runtime.msgq.chassis_imu); // 重置消息队列，防止阻塞 + // chassis_imu_send.accl = dm_imu.data.accl; + // chassis_imu_send.gyro = dm_imu.data.gyro; + // chassis_imu_send.euler = dm_imu.data.euler; + // osMessageQueuePut(task_runtime.msgq.chassis_imu, &chassis_imu_send, 0, 0); // 非阻塞发送IMU数据 + // } /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } diff --git a/User/task/ctrl_chassis.c b/User/task/ctrl_chassis.c index 458a799..199e707 100644 --- a/User/task/ctrl_chassis.c +++ b/User/task/ctrl_chassis.c @@ -9,6 +9,7 @@ #include "bsp/can.h" #include "device/motor_lk.h" #include "device/motor_lz.h" +#include "device/virtual_chassis.h" #include "module/config.h" #include "module/balance_chassis.h" @@ -30,7 +31,7 @@ Chassis_CMD_t chassis_cmd = { .height = 0.0f, }; Chassis_IMU_t chassis_imu; -Chassis_t chassis1; +Virtual_Chassis_t virtual_chassis; // 添加虚拟底盘设备 /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ @@ -48,23 +49,60 @@ void Task_ctrl_chassis(void *argument) { /* USER CODE INIT BEGIN */ Chassis_Init(&chassis, &Config_GetRobotParam()->chassis_param, CTRL_CHASSIS_FREQ); - + + // 初始化虚拟底盘设备 + Virtual_Chassis_Init(&virtual_chassis, &Config_GetRobotParam()->virtual_chassis_param); + Virtual_Chassis_Enable(&virtual_chassis); // 使能虚拟底盘 /* USER CODE INIT END */ while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ - if (osMessageQueueGet(task_runtime.msgq.chassis_imu, &chassis_imu, NULL, 0) == osOK) { + // 更新虚拟底盘数据 + Virtual_Chassis_Update(&virtual_chassis); + + // 从虚拟底盘获取IMU数据 + Virtual_Chassis_IMUFeedback_t *virtual_imu = Virtual_Chassis_GetIMUFeedback(&virtual_chassis); + if (virtual_imu != NULL && virtual_imu->online) { + chassis_imu.accl = virtual_imu->accl; + chassis_imu.gyro = virtual_imu->gyro; + chassis_imu.euler = virtual_imu->euler; chassis.feedback.imu = chassis_imu; + } else { + // 如果虚拟底盘IMU离线，从消息队列获取IMU数据作为备用 + if (osMessageQueueGet(task_runtime.msgq.chassis_imu, &chassis_imu, NULL, 0) == osOK) { + chassis.feedback.imu = chassis_imu; + } } + + // 从虚拟底盘获取关节电机数据 + for (int i = 0; i < 4; i++) { + Virtual_Chassis_MotorFeedback_t *virtual_motor = Virtual_Chassis_GetMotorFeedback(&virtual_chassis, i); + if (virtual_motor != NULL && virtual_motor->online) { + // 将虚拟底盘的电机数据转换为底盘反馈格式 + chassis.feedback.joint[i].rotor_abs_angle = virtual_motor->current_angle; + chassis.feedback.joint[i].rotor_speed = virtual_motor->current_velocity; + chassis.feedback.joint[i].torque_current = virtual_motor->current_torque; + chassis.feedback.joint[i].temp = 25.0f; // 默认温度 + } + } + if (osMessageQueueGet(task_runtime.msgq.Chassis_cmd, &chassis_cmd, NULL, 0) == osOK) { // 成功接收到命令，更新底盘命令 } Chassis_UpdateFeedback(&chassis); Chassis_Control(&chassis, &chassis_cmd); - + + // 将控制输出发送给虚拟底盘 + if (Virtual_Chassis_IsOnline(&virtual_chassis)) { + float torques[4]; + for (int i = 0; i < 4; i++) { + torques[i] = chassis.output.joint[i].torque; // 从底盘控制输出获取力矩 + } + // Virtual_Chassis_SendTorqueCommand(&virtual_chassis, torques); + } /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ diff --git a/User/task/ctrl_virtual_chassis.c b/User/task/ctrl_virtual_chassis.c new file mode 100644 index 0000000..daf970f --- /dev/null +++ b/User/task/ctrl_virtual_chassis.c @@ -0,0 +1,98 @@ +/* + ctrl_virtual_chassis Task - 使用虚拟底盘设备的示例任务 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" +/* USER INCLUDE BEGIN */ +#include "device/virtual_chassis.h" +#include "module/config.h" +#include "bsp/time.h" +/* USER INCLUDE END */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* USER STRUCT BEGIN */ +static Virtual_Chassis_t virtual_chassis; +static bool chassis_enabled = false; +static float test_torques[VIRTUAL_CHASSIS_MOTOR_COUNT] = {0.0f, 0.0f, 0.0f, 0.0f}; +/* USER STRUCT END */ + +/* Private function --------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +void Task_ctrl_virtual_chassis(void *argument) { + (void)argument; /* 未使用argument，消除警告 */ + + + /* 计算任务运行到指定频率需要等待的tick数 */ + const uint32_t delay_tick = osKernelGetTickFreq() / 100.0f; // 100Hz频率 + + osDelay(500); /* 延时一段时间再开启任务 */ + + uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ + /* USER CODE INIT BEGIN */ + + // 初始化虚拟底盘 + if (Virtual_Chassis_Init(&virtual_chassis, &Config_GetRobotParam()->virtual_chassis_param) != DEVICE_OK) { + // 初始化失败处理 + return; + } + + // 使能底盘电机 + Virtual_Chassis_Enable(&virtual_chassis); + chassis_enabled = true; + + /* USER CODE INIT END */ + + while (1) { + tick += delay_tick; /* 计算下一个唤醒时刻 */ + /* USER CODE BEGIN */ + + // 更新虚拟底盘数据（接收反馈） + Virtual_Chassis_Update(&virtual_chassis); + + // 检查设备在线状态 + if (Virtual_Chassis_IsOnline(&virtual_chassis)) { + + // 发送力矩控制命令（这里可以根据实际需要设置力矩值） + if (chassis_enabled) { + Virtual_Chassis_SendTorqueCommand(&virtual_chassis, test_torques); + } + + // 获取电机反馈数据 + for (int i = 0; i < VIRTUAL_CHASSIS_MOTOR_COUNT; i++) { + Virtual_Chassis_MotorFeedback_t *motor_fb = Virtual_Chassis_GetMotorFeedback(&virtual_chassis, i); + if (motor_fb != NULL && motor_fb->online) { + // 这里可以使用电机反馈数据 + // 例如：发送到其他任务或用于控制算法 + // motor_fb->current_angle, motor_fb->current_velocity, motor_fb->current_torque + } + } + + // 获取IMU反馈数据 + Virtual_Chassis_IMUFeedback_t *imu_fb = Virtual_Chassis_GetIMUFeedback(&virtual_chassis); + if (imu_fb != NULL && imu_fb->online) { + // 这里可以使用IMU数据替换原有的陀螺仪数据 + // 例如：将数据发送给底盘控制任务 + Chassis_IMU_t chassis_imu = { + .accl = imu_fb->accl, + .gyro = imu_fb->gyro, + .euler = imu_fb->euler, + }; + + // 发送IMU数据到底盘控制任务 + osMessageQueuePut(task_runtime.msgq.chassis_imu, &chassis_imu, 0, 0); + } + } else { + // 设备离线，重新使能 + if (chassis_enabled) { + Virtual_Chassis_Enable(&virtual_chassis); + } + } + + /* USER CODE END */ + osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ + } +} \ No newline at end of file