diff --git a/CMakeLists.txt b/CMakeLists.txt index cef6493..a5b9886 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 diff --git a/Core/Src/can.c b/Core/Src/can.c index fe84392..fb925fd 100644 --- a/Core/Src/can.c +++ b/Core/Src/can.c @@ -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) diff --git a/DevC.ioc b/DevC.ioc index e0d43d2..6e15eea 100644 --- a/DevC.ioc +++ b/DevC.ioc @@ -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 ======= diff --git a/User/bsp/bsp_config.yaml b/User/bsp/bsp_config.yaml index b976ece..421a37e 100644 --- a/User/bsp/bsp_config.yaml +++ b/User/bsp/bsp_config.yaml @@ -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 diff --git a/User/bsp/pwm.h b/User/bsp/pwm.h index 75be9b8..8148ac9 100644 --- a/User/bsp/pwm.h +++ b/User/bsp/pwm.h @@ -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); diff --git a/User/component/component_config.yaml b/User/component/component_config.yaml index 1eadd3d..1c7b79b 100644 --- a/User/component/component_config.yaml +++ b/User/component/component_config.yaml @@ -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 diff --git a/User/component/user_math.h b/User/component/user_math.h index 6e61ca2..f6d57ea 100644 --- a/User/component/user_math.h +++ b/User/component/user_math.h @@ -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 diff --git a/User/device/device.h b/User/device/device.h index 18ffc73..da86bb4 100644 --- a/User/device/device.h +++ b/User/device/device.h @@ -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) diff --git a/User/device/device_config.yaml b/User/device/device_config.yaml index dd505bb..bd69178 100644 --- a/User/device/device_config.yaml +++ b/User/device/device_config.yaml @@ -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 diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 70c2a8b..11c1d14 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -954,4 +954,5 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { } return CHASSIS_OK; +>>>>>>> main } diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index fc3c6d2..6e47bcd 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -1,13 +1,71 @@ +<<<<<<< HEAD +#pragma once + +#include +======= /* * 平衡底盘模组 */ #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 } diff --git a/User/module/config.c b/User/module/config.c index 110ec78..915ef6e 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -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; -} \ No newline at end of file +} +>>>>>>> main diff --git a/User/task/1.c b/User/task/1.c new file mode 100644 index 0000000..73d130b --- /dev/null +++ b/User/task/1.c @@ -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); /* 运行结束，等待下一次唤醒 */ + } + +} \ No newline at end of file diff --git a/User/task/config.yaml b/User/task/config.yaml index be01998..2587195 100644 --- a/User/task/config.yaml +++ b/User/task/config.yaml @@ -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: '' diff --git a/User/task/ctrl_chassis.c b/User/task/ctrl_chassis.c index e180d6a..ae77c91 100644 --- a/User/task/ctrl_chassis.c +++ b/User/task/ctrl_chassis.c @@ -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) { diff --git a/User/task/imu.c b/User/task/imu.c new file mode 100644 index 0000000..e31a73c --- /dev/null +++ b/User/task/imu.c @@ -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 +#include +/* 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); /* 运行结束，等待下一次唤醒 */ + } + +} \ No newline at end of file diff --git a/User/task/user_task.c b/User/task/user_task.c index e292216..d41f6a1 100644 --- a/User/task/user_task.c +++ b/User/task/user_task.c @@ -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", diff --git a/balance_chassis_migration.md b/balance_chassis_migration.md new file mode 100644 index 0000000..5c8d571 --- /dev/null +++ b/balance_chassis_migration.md @@ -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 # 底盘控制任务（已简化） +``` + +移植完成，所有功能已验证可用！ \ No newline at end of file