diff --git a/User/device/virtual_chassis.c b/User/device/virtual_chassis.c index 118cd81..c21cb3a 100644 --- a/User/device/virtual_chassis.c +++ b/User/device/virtual_chassis.c @@ -9,23 +9,37 @@ #include "bsp/can.h" #include "bsp/time.h" #include +#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) { +/** + * @brief 解析关节电机反馈数据 (ID: 124-127) + * @param chassis 虚拟底盘结构体 + * @param id CAN ID + * @param data 数据包 + * @return 解析结果 + */ +static int8_t Virtual_Chassis_DecodeJointMotor(Virtual_Chassis_t *chassis, uint16_t id, uint8_t *data) { if (chassis == NULL || data == NULL) { return DEVICE_ERR_NULL; } + int motor_idx = id - chassis->param.motors.joint_feedback_base_id; + if (motor_idx < 0 || motor_idx >= 4) { + return DEVICE_ERR; + } + + // 解析关节电机反馈数据:转矩电流(2字节) + 位置(3字节) + 速度(3字节) + // 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; + chassis->data.joint[motor_idx].torque_current = (float)torque_int / 100.0f; // 2. 解析位置 - 第3-5字节,24位有符号整数 (精度0.0001 rad) int32_t angle_int = 0; @@ -37,7 +51,7 @@ static int8_t Virtual_Chassis_DecodeMotor(Virtual_Chassis_t *chassis, uint8_t id if (angle_int & 0x800000) { angle_int |= 0xFF000000; } - chassis->data.joint[id-chassis->param.motors.feedback_id].rotor_abs_angle = (float)angle_int / 10000.0f; + chassis->data.joint[motor_idx].rotor_abs_angle = (float)angle_int / 10000.0f; // 3. 解析速度 - 第6-8字节,24位有符号整数 (精度0.001 rad/s) int32_t velocity_int = 0; @@ -49,63 +63,110 @@ static int8_t Virtual_Chassis_DecodeMotor(Virtual_Chassis_t *chassis, uint8_t id 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[motor_idx].rotor_speed = (float)velocity_int / 1000.0f; - chassis->data.joint[id-chassis->param.motors.feedback_id].temp = 0.0f; + chassis->data.joint[motor_idx].temp = 0.0f; return DEVICE_OK; } -static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint8_t id, uint8_t *data) { +/** + * @brief 解析轮子电机反馈数据 (ID: 130-131) + * @param chassis 虚拟底盘结构体 + * @param id CAN ID + * @param data 数据包 + * @return 解析结果 + */ +static int8_t Virtual_Chassis_DecodeWheelMotor(Virtual_Chassis_t *chassis, uint16_t id, uint8_t *data) { if (chassis == NULL || data == NULL) { return DEVICE_ERR_NULL; } - switch (id) { - case 150: { // 加速度计数据 - x/y/z 各2字节,精度0.01g - int16_t x, y, z; - memcpy(&x, &data[0], 2); - memcpy(&y, &data[2], 2); - memcpy(&z, &data[4], 2); - chassis->data.imu.accl.x = x / 100.0f; - chassis->data.imu.accl.y = y / 100.0f; - chassis->data.imu.accl.z = z / 100.0f; - break; - } - case 151: { // 陀螺仪数据 - x/y/z 各2字节,精度0.01°/s,需转回弧度/s - int16_t x, y, z; - memcpy(&x, &data[0], 2); - memcpy(&y, &data[2], 2); - memcpy(&z, &data[4], 2); - chassis->data.imu.gyro.x = (x / 100.0f) * 0.0174533f; // °/s -> rad/s - chassis->data.imu.gyro.y = (y / 100.0f) * 0.0174533f; - chassis->data.imu.gyro.z = (z / 100.0f) * 0.0174533f; - break; - } - case 152: { // 欧拉角数据 - yaw/pitch/roll 各2字节,精度0.01°,需转回弧度 - int16_t yaw, pit, rol; - memcpy(&yaw, &data[0], 2); - memcpy(&pit, &data[2], 2); - memcpy(&rol, &data[4], 2); - chassis->data.imu.euler.yaw = (yaw / 100.0f) * 0.0174533f; // ° -> rad - chassis->data.imu.euler.pit = (pit / 100.0f) * 0.0174533f; - chassis->data.imu.euler.rol = (rol / 100.0f) * 0.0174533f; - break; - } - case 153: { // 四元数数据 - q0/q1/q2/q3 各2字节,精度0.0001 - int16_t q0, q1, q2, q3; - memcpy(&q0, &data[0], 2); - memcpy(&q1, &data[2], 2); - memcpy(&q2, &data[4], 2); - memcpy(&q3, &data[6], 2); - chassis->data.imu.quat.q0 = q0 / 10000.0f; - chassis->data.imu.quat.q1 = q1 / 10000.0f; - chassis->data.imu.quat.q2 = q2 / 10000.0f; - chassis->data.imu.quat.q3 = q3 / 10000.0f; - break; - } - default: - return DEVICE_ERR; + int wheel_idx; + if (id == chassis->param.motors.wheel_left_feedback_id) { + wheel_idx = 0; // 左轮 + } else if (id == chassis->param.motors.wheel_right_feedback_id) { + wheel_idx = 1; // 右轮 + } else { + return DEVICE_ERR; + } + + // 解析轮子电机反馈数据:角度(2字节) + 速度(2字节) + 力矩电流(2字节) + 编码器(2字节) + + // 角度 - 精度0.01度,需转换为弧度 + int16_t angle_int; + angle_int = (data[0] << 8) | data[1]; + chassis->data.wheel[wheel_idx].rotor_abs_angle = (float)angle_int / 100.0f * M_PI / 180.0f; + + // 速度 - 精度1dps,直接赋值 + int16_t speed_int; + speed_int = (data[2] << 8) | data[3]; + chassis->data.wheel[wheel_idx].rotor_speed = (float)speed_int; + + // 力矩电流 + int16_t current_int; + current_int = (data[4] << 8) | data[5]; + chassis->data.wheel[wheel_idx].torque_current = (float)current_int; + + // 编码器值(暂不使用) + chassis->data.wheel[wheel_idx].temp = 0.0f; + + return DEVICE_OK; +} + +/** + * @brief 解析IMU数据 + * @param chassis 虚拟底盘结构体 + * @param id CAN ID + * @param data 数据包 + * @return 解析结果 + */ +static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint16_t id, uint8_t *data) { + if (chassis == NULL || data == NULL) { + return DEVICE_ERR_NULL; + } + + if (id == chassis->param.imu.accl_id) { // 加速度计数据 - x/y/z 各2字节,精度0.01g + int16_t x, y, z; + memcpy(&x, &data[0], 2); + memcpy(&y, &data[2], 2); + memcpy(&z, &data[4], 2); + chassis->data.imu.accl.x = x / 100.0f; + chassis->data.imu.accl.y = y / 100.0f; + chassis->data.imu.accl.z = z / 100.0f; + } + else if (id == chassis->param.imu.gyro_id) { // 陀螺仪数据 - x/y/z 各2字节,精度0.01°/s + int16_t x, y, z; + memcpy(&x, &data[0], 2); + memcpy(&y, &data[2], 2); + memcpy(&z, &data[4], 2); + // 注意:底盘发送的是精度0.01°/s,而不是0.1°/s,所以除以100而不是10 + chassis->data.imu.gyro.x = (x / 100.0f) * M_PI / 180.0f; // °/s -> rad/s + chassis->data.imu.gyro.y = (y / 100.0f) * M_PI / 180.0f; + chassis->data.imu.gyro.z = (z / 100.0f) * M_PI / 180.0f; + } + else if (id == chassis->param.imu.euler_id) { // 欧拉角数据 - yaw/pitch/roll 各2字节,精度0.01° + int16_t yaw, pit, rol; + memcpy(&yaw, &data[0], 2); + memcpy(&pit, &data[2], 2); + memcpy(&rol, &data[4], 2); + chassis->data.imu.euler.yaw = (yaw / 100.0f) * M_PI / 180.0f; // ° -> rad + chassis->data.imu.euler.pit = (pit / 100.0f) * M_PI / 180.0f; + chassis->data.imu.euler.rol = (rol / 100.0f) * M_PI / 180.0f; + } + else if (id == chassis->param.imu.quat_id) { // 四元数数据 - q0/q1/q2/q3 各2字节,精度1/32000 + int16_t q0, q1, q2, q3; + memcpy(&q0, &data[0], 2); + memcpy(&q1, &data[2], 2); + memcpy(&q2, &data[4], 2); + memcpy(&q3, &data[6], 2); + chassis->data.imu.quat.q0 = q0 / 32000.0f; + chassis->data.imu.quat.q1 = q1 / 32000.0f; + chassis->data.imu.quat.q2 = q2 / 32000.0f; + chassis->data.imu.quat.q3 = q3 / 32000.0f; + } + else { + return DEVICE_ERR; } return DEVICE_OK; @@ -134,11 +195,21 @@ int8_t Virtual_Chassis_Init(Virtual_Chassis_t *chassis, Virtual_Chassis_Param_t BSP_CAN_Init(); + // 注册关节电机反馈ID (124-127) 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, 150 + i, 3); + BSP_CAN_RegisterId(chassis->param.motors.can, chassis->param.motors.joint_feedback_base_id + i, 3); } + // 注册轮子电机反馈ID (130-131) + BSP_CAN_RegisterId(chassis->param.motors.can, chassis->param.motors.wheel_left_feedback_id, 3); + BSP_CAN_RegisterId(chassis->param.motors.can, chassis->param.motors.wheel_right_feedback_id, 3); + + // 注册IMU数据ID (769-772 / 0x301-0x304) + BSP_CAN_RegisterId(chassis->param.imu.can, chassis->param.imu.accl_id, 3); + BSP_CAN_RegisterId(chassis->param.imu.can, chassis->param.imu.gyro_id, 3); + BSP_CAN_RegisterId(chassis->param.imu.can, chassis->param.imu.euler_id, 3); + BSP_CAN_RegisterId(chassis->param.imu.can, chassis->param.imu.quat_id, 3); + // 设置设备在线状态 chassis->header.online = true; chassis->header.last_online_time = BSP_TIME_Get_us(); @@ -157,24 +228,35 @@ int8_t Virtual_Chassis_Update(Virtual_Chassis_t *chassis) { BSP_CAN_Message_t msg; - for (int i = 0; i < 4; i++){ - if(BSP_CAN_GetMessage(chassis->param.imu.can, 150 + i, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK){ - continue; - } else { - Virtual_Chassis_DecodeIMU(chassis, 150 + i, msg.data); + // 接收IMU数据 + if (BSP_CAN_GetMessage(chassis->param.imu.can, chassis->param.imu.accl_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + Virtual_Chassis_DecodeIMU(chassis, chassis->param.imu.accl_id, msg.data); + } + if (BSP_CAN_GetMessage(chassis->param.imu.can, chassis->param.imu.gyro_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + Virtual_Chassis_DecodeIMU(chassis, chassis->param.imu.gyro_id, msg.data); + } + if (BSP_CAN_GetMessage(chassis->param.imu.can, chassis->param.imu.euler_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + Virtual_Chassis_DecodeIMU(chassis, chassis->param.imu.euler_id, msg.data); + } + if (BSP_CAN_GetMessage(chassis->param.imu.can, chassis->param.imu.quat_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + Virtual_Chassis_DecodeIMU(chassis, chassis->param.imu.quat_id, msg.data); + } + + // 接收关节电机反馈数据 (124-127) + for (int i = 0; i < 4; i++) { + uint16_t joint_id = chassis->param.motors.joint_feedback_base_id + i; + if (BSP_CAN_GetMessage(chassis->param.motors.can, joint_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + Virtual_Chassis_DecodeJointMotor(chassis, joint_id, msg.data); } } - for (int i = 0; i < 4; i++){ - if (BSP_CAN_GetMessage(chassis->param.motors.can, 124 + i, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK){ - continue; - } else { - // 解析电机反馈数据 - Virtual_Chassis_DecodeMotor(chassis, 124 + i, msg.data); - } + // 接收轮子电机反馈数据 (130-131) + if (BSP_CAN_GetMessage(chassis->param.motors.can, chassis->param.motors.wheel_left_feedback_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + Virtual_Chassis_DecodeWheelMotor(chassis, chassis->param.motors.wheel_left_feedback_id, msg.data); + } + if (BSP_CAN_GetMessage(chassis->param.motors.can, chassis->param.motors.wheel_right_feedback_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + Virtual_Chassis_DecodeWheelMotor(chassis, chassis->param.motors.wheel_right_feedback_id, msg.data); } - - return DEVICE_OK; } @@ -189,9 +271,13 @@ int8_t Virtual_Chassis_Enable(Virtual_Chassis_t *chassis) { } BSP_CAN_StdDataFrame_t enable_frame = { - .id = chassis->param.motors.Enable_id, + .id = chassis->param.motors.enable_id, + .dlc = 0, + .data = {0} }; + chassis->output.enable_joints = true; + if (BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &enable_frame) == BSP_OK) { return DEVICE_OK; } @@ -200,15 +286,15 @@ int8_t Virtual_Chassis_Enable(Virtual_Chassis_t *chassis) { } /** - * @brief 发送电机力矩控制命令 + * @brief 发送关节电机力矩控制命令 */ -int8_t Virtual_Chassis_SendTorqueCommand(Virtual_Chassis_t *chassis, float torques[4]) { +int8_t Virtual_Chassis_SendJointTorque(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, + .id = chassis->param.motors.joint_cmd_id, .dlc = 8, .data = {0} }; @@ -223,5 +309,46 @@ int8_t Virtual_Chassis_SendTorqueCommand(Virtual_Chassis_t *chassis, float torqu return BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &torque_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; } +/** + * @brief 发送轮子电机控制命令 + */ +int8_t Virtual_Chassis_SendWheelCommand(Virtual_Chassis_t *chassis, uint8_t left_cmd[8], uint8_t right_cmd[8]) { + if (chassis == NULL || left_cmd == NULL || right_cmd == NULL) { + return DEVICE_ERR_NULL; + } + + // 发送左轮控制命令 + BSP_CAN_StdDataFrame_t left_frame = { + .id = chassis->param.motors.wheel_left_id, + .dlc = 8, + }; + memcpy(left_frame.data, left_cmd, 8); + memcpy(chassis->output.wheel_left_cmd, left_cmd, 8); + + // 发送右轮控制命令 + BSP_CAN_StdDataFrame_t right_frame = { + .id = chassis->param.motors.wheel_right_id, + .dlc = 8, + }; + memcpy(right_frame.data, right_cmd, 8); + memcpy(chassis->output.wheel_right_cmd, right_cmd, 8); + + int8_t result1 = BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &left_frame); + int8_t result2 = BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &right_frame); + + return (result1 == BSP_OK && result2 == BSP_OK) ? DEVICE_OK : DEVICE_ERR; +} + +/** + * @brief 检查虚拟底盘是否在线 + */ +bool Virtual_Chassis_IsOnline(Virtual_Chassis_t *chassis) { + if (chassis == NULL) { + return false; + } + + return chassis->header.online; +} + /* Private functions -------------------------------------------------------- */ diff --git a/User/device/virtual_chassis.h b/User/device/virtual_chassis.h index ad36e60..3b6eddd 100644 --- a/User/device/virtual_chassis.h +++ b/User/device/virtual_chassis.h @@ -16,37 +16,46 @@ extern "C" { /* Exported macro ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */ -/* 电机反馈数据结构体 */ +/* 虚拟底盘反馈数据结构体 */ typedef struct { - MOTOR_Feedback_t joint[4]; // 4个电机反馈数据 + MOTOR_Feedback_t joint[4]; // 4个关节电机反馈数据 + MOTOR_Feedback_t wheel[2]; // 2个轮子电机反馈数据 struct { - DEVICE_Header_t header; // 设备通用头部 - AHRS_Accl_t accl; // 加速度计数据 - AHRS_Gyro_t gyro; // 陀螺仪数据 - AHRS_Eulr_t euler; // 欧拉角数据 - AHRS_Quaternion_t quat; // 四元数数据 + 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个电机目标力矩 + float joint_torque[4]; // 4个关节电机目标力矩 + uint8_t wheel_left_cmd[8]; // 左轮控制命令数据 + uint8_t wheel_right_cmd[8]; // 右轮控制命令数据 + bool enable_joints; // 关节使能标志 } 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 + uint16_t enable_id; // 电机使能命令CAN ID (121) + uint16_t joint_cmd_id; // 关节力矩控制命令CAN ID (122) + uint16_t wheel_left_id; // 左轮控制命令CAN ID (128) + uint16_t wheel_right_id; // 右轮控制命令CAN ID (129) + uint16_t joint_feedback_base_id; // 关节反馈基础ID (124-127) + uint16_t wheel_left_feedback_id; // 左轮反馈ID (130) + uint16_t wheel_right_feedback_id; // 右轮反馈ID (131) } 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 + BSP_CAN_t can; // IMU所在CAN总线 + uint16_t accl_id; // 加速度计数据CAN ID (0x301/769) + uint16_t gyro_id; // 陀螺仪数据CAN ID (0x302/770) + uint16_t euler_id; // 欧拉角数据CAN ID (0x303/771) + uint16_t quat_id; // 四元数数据CAN ID (0x304/772) } Virtual_Chassis_IMUParam_t; /* 虚拟底盘参数配置结构体 */ @@ -88,11 +97,21 @@ int8_t Virtual_Chassis_Update(Virtual_Chassis_t *chassis); int8_t Virtual_Chassis_Enable(Virtual_Chassis_t *chassis); /** - * @brief 使能电机并发送命令 + * @brief 发送关节电机力矩控制命令 * @param chassis 虚拟底盘设备结构体指针 + * @param torques 4个关节电机的目标力矩数组 * @return DEVICE_OK 成功,其他值失败 */ -int8_t Virtual_Chassis_Enable(Virtual_Chassis_t *chassis); +int8_t Virtual_Chassis_SendJointTorque(Virtual_Chassis_t *chassis, float torques[4]); + +/** + * @brief 发送轮子电机控制命令 + * @param chassis 虚拟底盘设备结构体指针 + * @param left_cmd 左轮控制命令数据(8字节) + * @param right_cmd 右轮控制命令数据(8字节) + * @return DEVICE_OK 成功,其他值失败 + */ +int8_t Virtual_Chassis_SendWheelCommand(Virtual_Chassis_t *chassis, uint8_t left_cmd[8], uint8_t right_cmd[8]); /** * @brief 检查虚拟底盘是否在线 diff --git a/User/module/config.c b/User/module/config.c index 1e67ff8..c716e68 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -187,16 +187,20 @@ Config_RobotParam_t robot_config = { .virtual_chassis_param = { .motors = { .can = BSP_CAN_1, - .Enable_id = 121, - .MIT_id = 0x122, - .feedback_id = 124, + .enable_id = 0x121, + .joint_cmd_id = 0x122, + .joint_feedback_base_id = 124, + .wheel_left_id = 0x128, + .wheel_right_id = 0x129, + .wheel_left_feedback_id = 0x82, + .wheel_right_feedback_id = 0x83, }, .imu = { .can = BSP_CAN_1, - .accl_id = 0x66, - .gyro_id = 0x67, - .euler_id = 0x68, - .quat_id = 0x69, + .accl_id = 0x301, + .gyro_id = 0x302, + .euler_id = 0x303, + .quat_id = 0x304, }, }, }; diff --git a/User/task/init.c b/User/task/init.c index e43ddc1..5de16ab 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -35,7 +35,7 @@ void Task_Init(void *argument) { task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink); task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc); task_runtime.thread.atti_esti = osThreadNew(Task_atti_esti, NULL, &attr_atti_esti); - // task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis); + task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis); // task_runtime.thread.ctrl_gimbal = osThreadNew(Task_ctrl_gimbal, NULL, &attr_ctrl_gimbal); // 创建消息队列 diff --git a/virtual_chassis_example.c b/virtual_chassis_example.c new file mode 100644 index 0000000..00bf209 --- /dev/null +++ b/virtual_chassis_example.c @@ -0,0 +1,95 @@ +/* + * @file virtual_chassis_example.c + * @brief 虚拟底盘设备使用示例 + * @details 展示如何配置和使用虚拟底盘与平衡底盘模块通信 + */ + +/* Includes ----------------------------------------------------------------- */ +#include "device/virtual_chassis.h" +#include "bsp/can.h" + +/* Private variables -------------------------------------------------------- */ +static Virtual_Chassis_t g_virtual_chassis; + +/* Example configuration ---------------------------------------------------- */ + +/** + * @brief 虚拟底盘配置示例 + */ +void Virtual_Chassis_Example_Init(void) { + // 配置虚拟底盘参数 + Virtual_Chassis_Param_t chassis_param = { + .motors = { + .can = BSP_CAN_1, // 电机使用CAN1总线 + .enable_id = 121, // 使能命令ID + .joint_cmd_id = 122, // 关节力矩控制命令ID + .wheel_left_id = 128, // 左轮控制命令ID + .wheel_right_id = 129, // 右轮控制命令ID + .joint_feedback_base_id = 124, // 关节反馈基础ID (124-127) + .wheel_left_feedback_id = 130, // 左轮反馈ID + .wheel_right_feedback_id = 131, // 右轮反馈ID + }, + .imu = { + .can = BSP_CAN_1, // IMU使用CAN1总线 + .accl_id = 0x301, // 加速度计数据ID (769) + .gyro_id = 0x302, // 陀螺仪数据ID (770) + .euler_id = 0x303, // 欧拉角数据ID (771) + .quat_id = 0x304, // 四元数数据ID (772) + } + }; + + // 初始化虚拟底盘 + Virtual_Chassis_Init(&g_virtual_chassis, &chassis_param); +} + +/** + * @brief 虚拟底盘控制示例 + */ +void Virtual_Chassis_Example_Control(void) { + // 1. 更新底盘数据(接收反馈) + Virtual_Chassis_Update(&g_virtual_chassis); + + // 2. 使能关节电机 + Virtual_Chassis_Enable(&g_virtual_chassis); + + // 3. 发送关节电机力矩控制命令 + float joint_torques[4] = {0.5f, -0.3f, 0.2f, -0.1f}; // 4个关节的目标力矩 (Nm) + Virtual_Chassis_SendJointTorque(&g_virtual_chassis, joint_torques); + + // 4. 发送轮子电机控制命令(示例:速度控制) + uint8_t left_wheel_cmd[8] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08}; // 左轮命令 + uint8_t right_wheel_cmd[8] = {0x08, 0x07, 0x06, 0x05, 0x04, 0x03, 0x02, 0x01}; // 右轮命令 + Virtual_Chassis_SendWheelCommand(&g_virtual_chassis, left_wheel_cmd, right_wheel_cmd); +} + +/** + * @brief 虚拟底盘数据读取示例 + */ +void Virtual_Chassis_Example_ReadData(void) { + // 读取关节电机反馈数据 + for (int i = 0; i < 4; i++) { + MOTOR_Feedback_t *joint = &g_virtual_chassis.data.joint[i]; + // 使用 joint->torque_current, joint->rotor_abs_angle, joint->rotor_speed + } + + // 读取轮子电机反馈数据 + for (int i = 0; i < 2; i++) { + MOTOR_Feedback_t *wheel = &g_virtual_chassis.data.wheel[i]; + // 使用 wheel->torque_current, wheel->rotor_abs_angle, wheel->rotor_speed + } + + // 读取IMU数据 + AHRS_Accl_t *accl = &g_virtual_chassis.data.imu.accl; + AHRS_Gyro_t *gyro = &g_virtual_chassis.data.imu.gyro; + AHRS_Eulr_t *euler = &g_virtual_chassis.data.imu.euler; + AHRS_Quaternion_t *quat = &g_virtual_chassis.data.imu.quat; + + // 使用IMU数据进行姿态估计或控制 +} + +/** + * @brief 获取虚拟底盘实例(供其他模块使用) + */ +Virtual_Chassis_t* Virtual_Chassis_GetInstance(void) { + return &g_virtual_chassis; +} \ No newline at end of file diff --git a/virtual_chassis_protocol.h b/virtual_chassis_protocol.h new file mode 100644 index 0000000..29c631e --- /dev/null +++ b/virtual_chassis_protocol.h @@ -0,0 +1,129 @@ +/* + * @file virtual_chassis_protocol.h + * @brief 虚拟底盘通信协议定义 + * @details 定义虚拟底盘与平衡底盘模块的CAN通信协议 + */ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* CAN通信协议定义 --------------------------------------------------------- */ + +/** + * 控制命令CAN ID定义(控制端→底盘端) + */ +#define VIRTUAL_CHASSIS_CAN_CMD_ENABLE 121 // 关节电机使能命令 +#define VIRTUAL_CHASSIS_CAN_CMD_JOINT 122 // 关节力矩控制命令 +#define VIRTUAL_CHASSIS_CAN_CMD_WHEEL_LEFT 128 // 左轮控制命令 +#define VIRTUAL_CHASSIS_CAN_CMD_WHEEL_RIGHT 129 // 右轮控制命令 + +/** + * 反馈数据CAN ID定义(底盘端→控制端) + */ +#define VIRTUAL_CHASSIS_CAN_FEEDBACK_JOINT_0 124 // 关节电机0反馈 +#define VIRTUAL_CHASSIS_CAN_FEEDBACK_JOINT_1 125 // 关节电机1反馈 +#define VIRTUAL_CHASSIS_CAN_FEEDBACK_JOINT_2 126 // 关节电机2反馈 +#define VIRTUAL_CHASSIS_CAN_FEEDBACK_JOINT_3 127 // 关节电机3反馈 +#define VIRTUAL_CHASSIS_CAN_FEEDBACK_WHEEL_LEFT 130 // 左轮电机反馈 +#define VIRTUAL_CHASSIS_CAN_FEEDBACK_WHEEL_RIGHT 131 // 右轮电机反馈 + +/** + * IMU数据CAN ID定义(底盘端→控制端) + */ +#define VIRTUAL_CHASSIS_CAN_IMU_ACCL 0x301 // 加速度计数据 (769) +#define VIRTUAL_CHASSIS_CAN_IMU_GYRO 0x302 // 陀螺仪数据 (770) +#define VIRTUAL_CHASSIS_CAN_IMU_EULER 0x303 // 欧拉角数据 (771) +#define VIRTUAL_CHASSIS_CAN_IMU_QUAT 0x304 // 四元数数据 (772) + +/* 数据格式定义 ----------------------------------------------------------- */ + +/** + * 关节电机控制命令数据格式 (ID: 122) + * 数据长度:8字节 + * 格式:4个电机的力矩值,每个电机2字节有符号整数 + * 精度:0.01 Nm + * 范围:-327.68 ~ +327.67 Nm + */ +typedef struct { + int16_t joint_torque[4]; // 关节电机目标力矩 * 100 +} Virtual_Chassis_JointCommand_t; + +/** + * 关节电机反馈数据格式 (ID: 124-127) + * 数据长度:8字节 + * 字节0-1:转矩电流 (精度0.01 Nm) + * 字节2-4:位置 (精度0.0001 rad,24位有符号) + * 字节5-7:速度 (精度0.001 rad/s,24位有符号) + */ +typedef struct { + int16_t torque_current; // 转矩电流 * 100 + int32_t position; // 位置 * 10000 (仅使用低24位) + int32_t velocity; // 速度 * 1000 (仅使用低24位) +} Virtual_Chassis_JointFeedback_t; + +/** + * 轮子电机反馈数据格式 (ID: 130-131) + * 数据长度:8字节 + * 字节0-1:角度 (精度0.01度) + * 字节2-3:速度 (精度1dps) + * 字节4-5:力矩电流 + * 字节6-7:编码器值 + */ +typedef struct { + int16_t angle_deg; // 角度 * 100 (度) + int16_t velocity_dps; // 速度 (度/秒) + int16_t torque_current; // 力矩电流 + uint16_t encoder; // 编码器值 +} Virtual_Chassis_WheelFeedback_t; + +/** + * 加速度计数据格式 (ID: 0x301) + * 数据长度:6字节 + * 精度:0.01g + */ +typedef struct { + int16_t x; // X轴加速度 * 100 + int16_t y; // Y轴加速度 * 100 + int16_t z; // Z轴加速度 * 100 +} Virtual_Chassis_AcclData_t; + +/** + * 陀螺仪数据格式 (ID: 0x302) + * 数据长度:6字节 + * 精度:0.01°/s + */ +typedef struct { + int16_t x; // X轴角速度 * 100 (°/s) + int16_t y; // Y轴角速度 * 100 (°/s) + int16_t z; // Z轴角速度 * 100 (°/s) +} Virtual_Chassis_GyroData_t; + +/** + * 欧拉角数据格式 (ID: 0x303) + * 数据长度:6字节 + * 精度:0.01° + */ +typedef struct { + int16_t yaw; // 偏航角 * 100 (°) + int16_t pitch; // 俯仰角 * 100 (°) + int16_t roll; // 横滚角 * 100 (°) +} Virtual_Chassis_EulerData_t; + +/** + * 四元数数据格式 (ID: 0x304) + * 数据长度:8字节 + * 精度:1/32000 + */ +typedef struct { + int16_t q0; // q0 * 32000 + int16_t q1; // q1 * 32000 + int16_t q2; // q2 * 32000 + int16_t q3; // q3 * 32000 +} Virtual_Chassis_QuatData_t; + +#ifdef __cplusplus +} +#endif \ No newline at end of file