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

1

Browse Source
This commit is contained in:
Robofish 2025-10-04 21:49:19 +08:00
parent ca97903bab
commit 088db5d94f
6 changed files with 67 additions and 365 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -76,6 +76,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/device/motor_lz.c
User/device/motor_rm.c
User/device/rc_can.c
User/device/virtual_chassis.c
# User/module sources
User/module/balance_chassis.c

295
User/device/virtual_chassis.c
View File

@ -60,39 +60,50 @@ static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint8_t id,
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;
case 0x66: { // 加速度计数据 - 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 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;
}
case 0x67: { // 陀螺仪数据 - 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 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;
}
case 0x68: { // 欧拉角数据 - 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 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;
}
case 0x69: { // 四元数数据 - 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;
}
@ -128,7 +139,6 @@ int8_t Virtual_Chassis_Init(Virtual_Chassis_t *chassis, Virtual_Chassis_Param_t
BSP_CAN_RegisterId(chassis->param.imu.can, 0x66 + 1, 3);
}
// 设置设备在线状态
chassis->header.online = true;
chassis->header.last_online_time = BSP_TIME_Get_us();
@ -146,30 +156,20 @@ int8_t Virtual_Chassis_Update(Virtual_Chassis_t *chassis) {
}
BSP_CAN_Message_t msg;
BSP_CAN_GetMessage(chassis->param.motors.can, 124, &msg, BSP_CAN_TIMEOUT_IMMEDIATE);
for (int i = 0; i < 4; i++){
BSP_CAN_GetMessage(chassis->param.motors.can, 124 + i, &msg, BSP_CAN_TIMEOUT_IMMEDIATE);
Virtual_Chassis_DecodeMotor(chassis, msg.parsed_id, msg.data);
}
for (int i = 0; i < 4; i++){
BSP_CAN_GetMessage(chassis->param.imu.can, 0x66 + i, &msg, BSP_CAN_TIMEOUT_IMMEDIATE);
Virtual_Chassis_DecodeIMU(chassis, msg.parsed_id, msg.data, msg.dlc);
}
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 发送电机使能命令
@ -214,204 +214,5 @@ 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 获取电机反馈数据
*/
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;
}

47
User/device/virtual_chassis.h
View File

@ -73,13 +73,6 @@ typedef struct {
*/
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 虚拟底盘设备结构体指针
@ -87,50 +80,12 @@ int8_t Virtual_Chassis_DeInit(Virtual_Chassis_t *chassis);
*/
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]);
int8_t Virtual_Chassis_Enable(Virtual_Chassis_t *chassis);
/**
* @brief 使能电机并发送命令

1
User/module/balance_chassis.h
View File

@ -25,6 +25,7 @@ extern "C" {
#include "device/motor_lk.h"
#include "device/motor_lz.h"
#include "device/motor_dm.h"
#include "device/virtual_chassis.h"
/* Exported constants ------------------------------------------------------- */
#define CHASSIS_OK (0) /* 运行正常 */
#define CHASSIS_ERR (-1) /* 运行时发现了其他错误 */

41
User/module/config.c
View File

@ -185,34 +185,19 @@ Config_RobotParam_t robot_config = {
},
.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,
.motors = {
.can = BSP_CAN_1,
.Enable_id = 121,
.MIT_id = 0x122,
.feedback_id = 124,
},
.imu = {
.can = BSP_CAN_1,
.accl_id = 0x66,
.gyro_id = 0x67,
.euler_id = 0x68,
.quat_id = 0x69,
},
},
};

47
User/task/ctrl_chassis.c
View File

@ -49,60 +49,19 @@ 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 */
// 更新虚拟底盘数据
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_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); /* 运行结束，等待下一次唤醒 */