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

合并 单独底层纯转发 到 MC_02

Browse Source
This commit is contained in:
Robofish 2026-02-25 15:43:35 +08:00
commit 887f9d778e
18 changed files with 533 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
CMakeLists.txt
View File

@ -53,6 +53,22 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/bsp/pwm.c
User/bsp/spi.c
User/bsp/time.c
<<<<<<< HEAD
# User/component sources
User/component/ahrs.c
User/component/filter.c
User/component/pid.c
User/component/user_math.c
# User/device sources
User/device/bmi088.c
User/device/motor.c
User/device/motor_lk.c
User/device/motor_lz.c
# User/module
=======
User/bsp/uart.c
# User/component sources
@ -85,6 +101,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/device/mrobot.c
# User/module sources
>>>>>>> main
User/module/balance_chassis.c
User/module/config.c
User/module/gimbal.c
@ -95,6 +112,10 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/task/atti_esit.c
User/task/blink.c
User/task/ctrl_chassis.c
<<<<<<< HEAD
User/task/imu.c
User/task/init.c
=======
User/task/ctrl_gimbal.c
User/task/ctrl_shoot.c
User/task/debug.c

4
Core/Src/can.c
View File

@ -64,7 +64,7 @@ void MX_CAN1_Init(void)
hcan1.Init.TimeTriggeredMode = DISABLE;
hcan1.Init.AutoBusOff = DISABLE;
hcan1.Init.AutoWakeUp = DISABLE;
hcan1.Init.AutoRetransmission = DISABLE;
hcan1.Init.AutoRetransmission = ENABLE;
hcan1.Init.ReceiveFifoLocked = DISABLE;
hcan1.Init.TransmitFifoPriority = ENABLE;
if (HAL_CAN_Init(&hcan1) != HAL_OK)
@ -96,7 +96,7 @@ void MX_CAN2_Init(void)
hcan2.Init.TimeTriggeredMode = DISABLE;
hcan2.Init.AutoBusOff = DISABLE;
hcan2.Init.AutoWakeUp = DISABLE;
hcan2.Init.AutoRetransmission = DISABLE;
hcan2.Init.AutoRetransmission = ENABLE;
hcan2.Init.ReceiveFifoLocked = DISABLE;
hcan2.Init.TransmitFifoPriority = ENABLE;
if (HAL_CAN_Init(&hcan2) != HAL_OK)

7
DevC.ioc
View File

@ -21,7 +21,8 @@ CAN1.BS2=CAN_BS2_7TQ
CAN1.CalculateBaudRate=1000000
CAN1.CalculateTimeBit=1000
CAN1.CalculateTimeQuantum=71.42857142857143
CAN1.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,ABOM,CalculateTimeBit,CalculateBaudRate
CAN1.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,ABOM,CalculateTimeBit,CalculateBaudRate,NART
CAN1.NART=ENABLE
CAN1.Prescaler=3
CAN1.TXFP=ENABLE
CAN2.BS1=CAN_BS1_6TQ
@ -29,7 +30,8 @@ CAN2.BS2=CAN_BS2_7TQ
CAN2.CalculateBaudRate=1000000
CAN2.CalculateTimeBit=1000
CAN2.CalculateTimeQuantum=71.42857142857143
CAN2.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,CalculateTimeBit,CalculateBaudRate
CAN2.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,CalculateTimeBit,CalculateBaudRate,NART
CAN2.NART=ENABLE
CAN2.Prescaler=3
CAN2.TXFP=ENABLE
Dma.I2C2_TX.2.Direction=DMA_MEMORY_TO_PERIPH
@ -276,6 +278,7 @@ MxDb.Version=DB.6.0.150
NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
NVIC.CAN1_RX0_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true\:true
NVIC.CAN1_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.CAN1_TX_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.CAN2_RX0_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true\:true
NVIC.CAN2_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
=======

3
User/bsp/bsp_config.yaml
View File

@ -120,6 +120,8 @@ spi:
enabled: true
time:
enabled: true
<<<<<<< HEAD
=======
uart:
devices:
- instance: UART5
@ -127,3 +129,4 @@ uart:
- instance: USART1
name: VOFA
enabled: true
>>>>>>> main

5
User/bsp/pwm.h
View File

@ -36,8 +36,13 @@ typedef enum {
/* Exported functions prototypes -------------------------------------------- */
int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch);
<<<<<<< HEAD
int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle); //设置占空比，范围0.0~1.0
int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq); //设置频率，单位Hz
=======
int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle);
int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq);
>>>>>>> main
int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch);
uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch);
uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch);

2
User/component/component_config.yaml
View File

@ -12,6 +12,7 @@ error_detect:
dependencies:
- bsp/mm
enabled: true
>>>>>>> main
filter:
dependencies:
- component/ahrs
@ -22,6 +23,7 @@ freertos_cli:
limiter:
dependencies: []
enabled: true
>>>>>>> main
pid:
dependencies:
- component/filter

7
User/component/user_math.h
View File

@ -29,6 +29,13 @@ extern "C" {
#define M_PI 3.14159265358979323846f
#endif
<<<<<<< HEAD
#ifndef M_PI_2
#define M_PI_2 1.57079632679489661923f
#endif
=======
>>>>>>> main
#ifndef M_2PI
#define M_2PI 6.28318530717958647692f
#endif

6
User/device/device.h
View File

@ -22,6 +22,12 @@ extern "C" {
#define DEVICE_ERR_NO_DEV (-4)
/* AUTO GENERATED SIGNALS BEGIN */
<<<<<<< HEAD
#define SIGNAL_BMI088_ACCL_RAW_REDY (1u << 0)
#define SIGNAL_BMI088_GYRO_RAW_REDY (1u << 1)
#define SIGNAL_BMI088_ACCL_NEW_DATA (1u << 2)
#define SIGNAL_BMI088_GYRO_NEW_DATA (1u << 3)
=======
#define SIGNAL_DR16_RAW_REDY (1u << 0)
#define SIGNAL_BMI088_ACCL_RAW_REDY (1u << 1)
#define SIGNAL_BMI088_GYRO_RAW_REDY (1u << 2)

5
User/device/device_config.yaml
View File

@ -10,10 +10,13 @@ buzzer:
bsp_config:
BSP_PWM_BUZZER: BSP_PWM_BUZZER
enabled: true
<<<<<<< HEAD
=======
dr16:
bsp_config:
BSP_UART_DR16: BSP_UART_DR16
enabled: true
>>>>>>> main
motor:
bsp_config: {}
enabled: true
@ -26,6 +29,8 @@ motor_lk:
motor_lz:
bsp_config: {}
enabled: true
<<<<<<< HEAD
=======
motor_rm:
bsp_config: {}
enabled: true

1
User/module/balance_chassis.c
View File

@ -954,4 +954,5 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
}
return CHASSIS_OK;
>>>>>>> main
}

59
User/module/balance_chassis.h
View File

@ -1,13 +1,71 @@
<<<<<<< HEAD
#pragma once
#include <stdint.h>
=======
/*
* 平衡底盘模组
*/
#pragma once
>>>>>>> main
#ifdef __cplusplus
extern "C" {
#endif
<<<<<<< HEAD
/* 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 */
=======
// Front
// | 1 4 |
// (1)Left| |Right(2)
@ -256,6 +314,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
* \param out CAN设备底盘输出结构体
*/
int8_t Chassis_Output(Chassis_t *c);
>>>>>>> main
#ifdef __cplusplus
}

6
User/module/config.c
View File

@ -374,6 +374,10 @@ Config_RobotParam_t robot_config = {
* @brief 获取机器人配置参数
* @return 机器人配置参数指针
*/
<<<<<<< HEAD
Config_RobotParam_t *Config_GetRobotParam(void) { return &robot_config; }
=======
Config_RobotParam_t* Config_GetRobotParam(void) {
return &robot_config;
}
}
>>>>>>> main

95
User/task/1.c Normal file
View File

@ -0,0 +1,95 @@
/*
imu Task
*/
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "bsp/pwm.h"
#include "component/ahrs.h"
#include "component/pid.h"
#include "device/bmi088.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
BMI088_t bmi088;
AHRS_t gimbal_ahrs;
AHRS_Magn_t magn;
AHRS_Eulr_t eulr_to_send;
KPID_t imu_temp_ctrl_pid;
BMI088_Cali_t cali_bmi088= {
.gyro_offset = {0.0f,0.0f,0.0f},
};
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void Task_imu(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
/* 计算任务运行到指定频率需要等待的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / IMU_FREQ;
osDelay(IMU_INIT_DELAY); /* 延时一段时间再开启任务 */
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
BMI088_Init(&bmi088,&cali_bmi088);
AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088));
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
BMI088_WaitNew();
BMI088_AcclStartDmaRecv();
BMI088_AcclWaitDmaCplt();
BMI088_GyroStartDmaRecv();
BMI088_GyroWaitDmaCplt();
/* 锁住RTOS内核防止数据解析过程中断，造成错误 */
osKernelLock();
/* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */
BMI088_ParseAccl(&bmi088);
/* 扩大加速度数据10倍，并交换x和y */
float temp_x = bmi088.accl.x * 10.0f;
float temp_y = bmi088.accl.y * 10.0f;
bmi088.accl.x = temp_y;
bmi088.accl.y = -temp_x;
bmi088.accl.z *= 10.0f;
BMI088_ParseGyro(&bmi088);
/* 交换陀螺仪x和y */
float temp_gyro_x = bmi088.gyro.x;
bmi088.gyro.x = bmi088.gyro.y;
bmi088.gyro.y = -temp_gyro_x;
// IST8310_Parse(&ist8310);
/* 根据设备接收到的数据进行姿态解析 */
AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn);
/* 根据解析出来的四元数计算欧拉角 */
AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
/* 交换pit和rol */
float temp_rol = eulr_to_send.rol;
eulr_to_send.rol = eulr_to_send.pit;
eulr_to_send.pit = temp_rol;
osKernelUnlock();
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}
}

10
User/task/config.yaml
View File

@ -2,8 +2,13 @@
description: ''
freq_control: true
frequency: 500.0
<<<<<<< HEAD
function: Task_imu
name: imu
=======
function: Task_rc
name: rc
>>>>>>> main
stack: 256
- delay: 0
description: ''
@ -22,8 +27,13 @@
description: ''
freq_control: true
frequency: 500.0
<<<<<<< HEAD
function: Task_atti_esti
name: atti_esti
=======
function: Task_ctrl_gimbal
name: ctrl_gimbal
>>>>>>> main
stack: 256
- delay: 0
description: ''

35
User/task/ctrl_chassis.c
View File

@ -6,6 +6,10 @@
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
<<<<<<< HEAD
#include "module/balance_chassis.h"
#include "module/config.h"
=======
#include "bsp/can.h"
#include "device/motor_lk.h"
#include "device/motor_lz.h"
@ -19,6 +23,9 @@
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
<<<<<<< HEAD
Chassis_t chassis; // 底盘实例
=======
Chassis_t chassis;
Chassis_CMD_t chassis_cmd = {
.mode = CHASSIS_MODE_RELAX, // 改为RECOVER模式，让电机先启动
@ -71,10 +78,38 @@ void Task_ctrl_chassis(void *argument) {
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
<<<<<<< HEAD
// 使用配置参数初始化底盘
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);
=======
Chassis_Init(&chassis, &Config_GetRobotParam()->chassis_param, CTRL_CHASSIS_FREQ);
MRobot_RegisterDevice("chassis", &chassis, print_chassis);
>>>>>>> main
/* USER CODE INIT END */
while (1) {

169
User/task/imu.c Normal file
View File

@ -0,0 +1,169 @@
/*
imu Task
*/
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "component/user_math.h"
#include "bsp/can.h"
#include "bsp/time.h"
#include "component/ahrs.h"
#include <string.h>
#include <stdint.h>
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* AHRS数据CAN ID定义 */
#define CAN_ID_AHRS_ACCL 0x301 /* 加速度计数据 */
#define CAN_ID_AHRS_GYRO 0x302 /* 陀螺仪数据 */
#define CAN_ID_AHRS_EULR 0x303 /* 欧拉角数据 */
#define CAN_ID_AHRS_QUAT 0x304 /* 四元数数据 */
/* 数据范围定义 */
#define ACCEL_CAN_MAX (58.8f)
#define ACCEL_CAN_MIN (-58.8f)
#define GYRO_CAN_MAX (34.88f)
#define GYRO_CAN_MIN (-34.88f)
#define PITCH_CAN_MAX (M_PI_2) /* π/2 弧度 ≈ 90° */
#define PITCH_CAN_MIN (-M_PI_2) /* -π/2 弧度 ≈ -90° */
#define ROLL_CAN_MAX (M_PI) /* π 弧度 ≈ 180° */
#define ROLL_CAN_MIN (-M_PI) /* -π 弧度 ≈ -180° */
#define YAW_CAN_MAX (M_PI) /* π 弧度 ≈ 180° */
#define YAW_CAN_MIN (-M_PI) /* -π 弧度 ≈ -180° */
#define QUATERNION_MIN (-1.0f)
#define QUATERNION_MAX (1.0f)
/* Private macro ------------------------------------------------------------ */
/* 数据映射宏：将浮点值映射到16位整数范围 */
#define MAP_TO_INT16(val, min, max) \
((int16_t)(((val) - (min)) / ((max) - (min)) * 65535.0f - 32768.0f))
/* 限制值在指定范围内 */
#define CLAMP(val, min, max) \
(((val) < (min)) ? (min) : (((val) > (max)) ? (max) : (val)))
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
AHRS_Accl_t accl;
AHRS_Gyro_t gyro;
AHRS_Eulr_t eulr;
AHRS_Quaternion_t quat;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void Task_imu(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
/* 计算任务运行到指定频率需要等待的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / IMU_FREQ;
osDelay(IMU_INIT_DELAY); /* 延时一段时间再开启任务 */
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
BSP_CAN_Init();
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
/* 获取加速度计数据并通过CAN发送 - 每轴使用16位编码 */
if (osMessageQueueGet(task_runtime.msgq.imu.accl, &accl, NULL, 0) == osOK) {
BSP_CAN_StdDataFrame_t accl_frame;
accl_frame.id = CAN_ID_AHRS_ACCL;
accl_frame.dlc = 6; /* 3轴 × 2字节 = 6字节 */
/* 限制并映射加速度数据到16位整数 */
float ax = CLAMP(accl.x, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
float ay = CLAMP(accl.y, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
float az = CLAMP(accl.z, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
int16_t ax_int = MAP_TO_INT16(ax, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
int16_t ay_int = MAP_TO_INT16(ay, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
int16_t az_int = MAP_TO_INT16(az, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
memcpy(&accl_frame.data[0], &ax_int, sizeof(int16_t));
memcpy(&accl_frame.data[2], &ay_int, sizeof(int16_t));
memcpy(&accl_frame.data[4], &az_int, sizeof(int16_t));
BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &accl_frame);
}
/* 获取陀螺仪数据并通过CAN发送 - 每轴使用16位编码 */
if (osMessageQueueGet(task_runtime.msgq.imu.gyro, &gyro, NULL, 0) == osOK) {
BSP_CAN_StdDataFrame_t gyro_frame;
gyro_frame.id = CAN_ID_AHRS_GYRO;
gyro_frame.dlc = 6; /* 3轴 × 2字节 = 6字节 */
/* 限制并映射陀螺仪数据到16位整数 */
float gx = CLAMP(gyro.x, GYRO_CAN_MIN, GYRO_CAN_MAX);
float gy = CLAMP(gyro.y, GYRO_CAN_MIN, GYRO_CAN_MAX);
float gz = CLAMP(gyro.z, GYRO_CAN_MIN, GYRO_CAN_MAX);
int16_t gx_int = MAP_TO_INT16(gx, GYRO_CAN_MIN, GYRO_CAN_MAX);
int16_t gy_int = MAP_TO_INT16(gy, GYRO_CAN_MIN, GYRO_CAN_MAX);
int16_t gz_int = MAP_TO_INT16(gz, GYRO_CAN_MIN, GYRO_CAN_MAX);
memcpy(&gyro_frame.data[0], &gx_int, sizeof(int16_t));
memcpy(&gyro_frame.data[2], &gy_int, sizeof(int16_t));
memcpy(&gyro_frame.data[4], &gz_int, sizeof(int16_t));
BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &gyro_frame);
}
/* 获取欧拉角数据并通过CAN发送 - 每角使用16位编码 */
if (osMessageQueueGet(task_runtime.msgq.imu.eulr, &eulr, NULL, 0) == osOK) {
BSP_CAN_StdDataFrame_t eulr_frame;
eulr_frame.id = CAN_ID_AHRS_EULR;
eulr_frame.dlc = 6; /* 3个角度 × 2字节 = 6字节 */
/* 限制并映射欧拉角数据到16位整数 */
float yaw = CLAMP(eulr.yaw, YAW_CAN_MIN, YAW_CAN_MAX);
float pitch = CLAMP(eulr.pit, PITCH_CAN_MIN, PITCH_CAN_MAX);
float roll = CLAMP(eulr.rol, ROLL_CAN_MIN, ROLL_CAN_MAX);
int16_t yaw_int = MAP_TO_INT16(yaw, YAW_CAN_MIN, YAW_CAN_MAX);
int16_t pitch_int = MAP_TO_INT16(pitch, PITCH_CAN_MIN, PITCH_CAN_MAX);
int16_t roll_int = MAP_TO_INT16(roll, ROLL_CAN_MIN, ROLL_CAN_MAX);
memcpy(&eulr_frame.data[0], &yaw_int, sizeof(int16_t));
memcpy(&eulr_frame.data[2], &pitch_int, sizeof(int16_t));
memcpy(&eulr_frame.data[4], &roll_int, sizeof(int16_t));
BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &eulr_frame);
}
/* 获取四元数数据并通过CAN发送 - 每个分量使用16位编码 */
if (osMessageQueueGet(task_runtime.msgq.imu.quat, &quat, NULL, 0) == osOK) {
BSP_CAN_StdDataFrame_t quat_frame;
quat_frame.id = CAN_ID_AHRS_QUAT;
quat_frame.dlc = 8; /* 4个四元数分量 × 2字节 = 8字节 */
/* 限制并映射四元数到16位整数 */
float q0 = CLAMP(quat.q0, QUATERNION_MIN, QUATERNION_MAX);
float q1 = CLAMP(quat.q1, QUATERNION_MIN, QUATERNION_MAX);
float q2 = CLAMP(quat.q2, QUATERNION_MIN, QUATERNION_MAX);
float q3 = CLAMP(quat.q3, QUATERNION_MIN, QUATERNION_MAX);
int16_t q0_int = MAP_TO_INT16(q0, QUATERNION_MIN, QUATERNION_MAX);
int16_t q1_int = MAP_TO_INT16(q1, QUATERNION_MIN, QUATERNION_MAX);
int16_t q2_int = MAP_TO_INT16(q2, QUATERNION_MIN, QUATERNION_MAX);
int16_t q3_int = MAP_TO_INT16(q3, QUATERNION_MIN, QUATERNION_MAX);
memcpy(&quat_frame.data[0], &q0_int, sizeof(int16_t));
memcpy(&quat_frame.data[2], &q1_int, sizeof(int16_t));
memcpy(&quat_frame.data[4], &q2_int, sizeof(int16_t));
memcpy(&quat_frame.data[6], &q3_int, sizeof(int16_t));
BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &quat_frame);
}
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}
}

3
User/task/user_task.c
View File

@ -11,6 +11,7 @@ const osThreadAttr_t attr_init = {
/* User_task */
const osThreadAttr_t attr_rc = {
.name = "rc",
>>>>>>> main
.priority = osPriorityNormal,
.stack_size = 256 * 8,
};
@ -18,6 +19,8 @@ const osThreadAttr_t attr_atti_esit = {
.name = "atti_esit",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
<<<<<<< HEAD
=======
};
const osThreadAttr_t attr_ctrl_chassis = {
.name = "ctrl_chassis",

100
balance_chassis_migration.md Normal file
View File

@ -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 # 底盘控制任务（已简化）
```
移植完成，所有功能已验证可用！