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

有反馈了

Browse Source
This commit is contained in:
Robofish 2025-10-05 12:04:46 +08:00
parent 722ea07411
commit 3b1ef030c2
6 changed files with 472 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

271
User/device/virtual_chassis.c
View File

@ -9,23 +9,37 @@
#include "bsp/can.h"
#include "bsp/time.h"
#include <string.h>
#include <math.h>
/* 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 -------------------------------------------------------- */

55
User/device/virtual_chassis.h
View File

@ -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 检查虚拟底盘是否在线

18
User/module/config.c
View File

@ -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,
},
},
};

2
User/task/init.c
View File

@ -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);
// 创建消息队列

95
virtual_chassis_example.c Normal file
View File

@ -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;
}

129
virtual_chassis_protocol.h Normal file
View File

@ -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