1.0
This commit is contained in:
@@ -2,55 +2,67 @@
|
||||
#define __LADRC_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include "motor.h" // 引入底层电机库以使用 Motor_ID_t 宏
|
||||
#include "motor.h"
|
||||
|
||||
/* LADRC 结构体定义 --------------------------------------------------------*/
|
||||
typedef struct {
|
||||
float v; // 目标速度 (Target)
|
||||
float y; // 实际速度 (Actual)
|
||||
/* 外部命令与内部参考 */
|
||||
float v_cmd; /* 上层下发的目标 RPM */
|
||||
float v; /* 内部斜坡后的参考 RPM */
|
||||
float y; /* 实际速度 RPM */
|
||||
|
||||
// LESO (线性扩展状态观测器) 状态变量
|
||||
float z1; // 系统输出的估计值 (估算的实际速度)
|
||||
float z2; // 总扰动的估计值 (包含所有未知的阻力和模型误差)
|
||||
/* LESO 状态 */
|
||||
float z1;
|
||||
float z2;
|
||||
|
||||
// 控制器核心参数 (仅需调这 3 个)
|
||||
float wc; // 控制器带宽 (Controller Bandwidth)
|
||||
float wo; // 观测器带宽 (Observer Bandwidth)
|
||||
float b0; // 系统的控制增益 (非常关键，决定了 PWM 到速度的转换系数)
|
||||
/* 核心参数 */
|
||||
float wc;
|
||||
float wo;
|
||||
float b0;
|
||||
|
||||
// LESO 内部增益
|
||||
float beta1; // 观测器增益 1
|
||||
float beta2; // 观测器增益 2
|
||||
/* LESO 增益 */
|
||||
float beta1;
|
||||
float beta2;
|
||||
|
||||
float h; // 采样周期 (单位：秒，比如 10ms 就是 0.01)
|
||||
float out_max; // 输出限幅 (如 1000)
|
||||
/* 采样与限幅 */
|
||||
float h;
|
||||
float out_max;
|
||||
|
||||
/* 控制输出 */
|
||||
float u;
|
||||
|
||||
/* Tustin 离散矩阵 */
|
||||
float Ad11;
|
||||
float Ad12;
|
||||
float Ad21;
|
||||
float Ad22;
|
||||
|
||||
float Bdu1;
|
||||
float Bdu2;
|
||||
|
||||
float Bdy1;
|
||||
float Bdy2;
|
||||
|
||||
/* Anti-windup: 作为 z2_dot 的额外输入，并离散到同一套 Tustin 模型 */
|
||||
float kaw;
|
||||
float eu_prev;
|
||||
float Bdaw1;
|
||||
float Bdaw2;
|
||||
|
||||
float u; // 最终计算出的控制量 (PWM)
|
||||
} LADRC_TypeDef;
|
||||
|
||||
/* 原有算法函数声明 --------------------------------------------------------*/
|
||||
void LADRC_Init(LADRC_TypeDef *ladrc, float wc, float wo, float b0, float h, float max);
|
||||
float LADRC_Calc(LADRC_TypeDef *ladrc, float actual_val);
|
||||
|
||||
/* ================= 针对四轮底盘的 LADRC 扩展调度层 ================= */
|
||||
|
||||
// 将 4 个控制器暴露出来，方便 main.c 里面调取数据用 VOFA+ 打印波形
|
||||
extern LADRC_TypeDef ladrc_motors[4];
|
||||
|
||||
/* 1. 初始化：一键初始化底层定时器和4个LADRC控制器 */
|
||||
void FourWheel_LADRC_Init(void);
|
||||
|
||||
/* 2. 设定目标转速：给四个轮子下发指令 */
|
||||
void FourWheel_Set_Target_RPM(float fl_rpm, float rl_rpm, float fr_rpm, float rr_rpm);
|
||||
|
||||
/* 3. 故障/停机时立即清零控制器内部状态并断驱 */
|
||||
void FourWheel_LADRC_ResetAll(void);
|
||||
|
||||
/* 4. 核心闭环运算：必须且只能在 10ms 基础定时器中断里调用 */
|
||||
void FourWheel_LADRC_Control_Loop(void);
|
||||
|
||||
/* 5. 诊断/状态上报接口：读取每个轮子的目标转速和控制输出 */
|
||||
float FourWheel_Get_Target_RPM(Motor_ID_t id);
|
||||
/* 命令目标与内部参考分开暴露，便于调试 */
|
||||
float FourWheel_Get_Target_RPM(Motor_ID_t id); /* 返回 v_cmd */
|
||||
float FourWheel_Get_Ref_RPM(Motor_ID_t id); /* 返回 v */
|
||||
float FourWheel_Get_Control_Output(Motor_ID_t id);
|
||||
|
||||
#endif /* __LADRC_H */
|
||||
#endif /* __LADRC_H */
|
||||
|
||||
@@ -3,42 +3,30 @@
|
||||
|
||||
#include "main.h"
|
||||
|
||||
/* ================== 硬件与物理参数配置 ================== */
|
||||
// PWM 满载占空比 (对应 CubeMX 中的 ARR = 1049)
|
||||
#define MAX_PWM_DUTY 1049
|
||||
#define MAX_PWM_DUTY 1049
|
||||
#define PWM_LIMIT 1000
|
||||
|
||||
// 为安全起见设置占空比上限，防溢出或满载锁死 (约 95%)
|
||||
#define PWM_LIMIT 1000
|
||||
#define ENCODER_RESOLUTION 11.0f
|
||||
#define REDUCTION_RATIO 90.0f
|
||||
#define PULSES_PER_REV (ENCODER_RESOLUTION * 4.0f * REDUCTION_RATIO)
|
||||
|
||||
// 编码器与减速箱参数 (JGB37-520 11线 90减速比)
|
||||
#define ENCODER_RESOLUTION 11.0f // 电机尾部编码器线数
|
||||
#define REDUCTION_RATIO 90.0f // 减速箱减速比 (90:1)
|
||||
// 车轮转一圈的总脉冲数 = 11 * 4(倍频) * 90 = 3960
|
||||
#define PULSES_PER_REV (ENCODER_RESOLUTION * 4.0f * REDUCTION_RATIO)
|
||||
/* 控制循环 5ms，但速度估计窗口取 2 个采样点 => 10ms */
|
||||
#define MOTOR_SPEED_WINDOW_SAMPLES 2U
|
||||
|
||||
/* ================== 枚举与类型定义 ================== */
|
||||
// 定义电机 ID (与物理位置一一对应)
|
||||
typedef enum {
|
||||
MOTOR_FL = 0, // 左上 Front-Left (PWM: TIM2, Encoder: TIM5)
|
||||
MOTOR_RL, // 左下 Rear-Left (PWM: TIM9, Encoder: TIM3)
|
||||
MOTOR_FR, // 右上 Front-Right (PWM: TIM8, Encoder: TIM4)
|
||||
MOTOR_RR // 右下 Rear-Right (PWM: TIM8, Encoder: TIM1)
|
||||
MOTOR_FL = 0,
|
||||
MOTOR_RL,
|
||||
MOTOR_FR,
|
||||
MOTOR_RR
|
||||
} Motor_ID_t;
|
||||
|
||||
/* ================== 函 数 声 明 ================== */
|
||||
// --- 基础控制层 ---
|
||||
void Motor_Init(void);
|
||||
// 设置电机输出 (正数代表期望车轮推动车体【向前】，负数【向后】)
|
||||
void Set_Motor_Output(Motor_ID_t id, int16_t control_out);
|
||||
// 紧急制动所有电机
|
||||
void Motor_Brake_All(void);
|
||||
|
||||
// --- 状态观测层 ---
|
||||
// 读取脉冲并更新转速 (需周期性调用，dt_s 为调用间隔的秒数，如 10ms 则传入 0.01f)
|
||||
/* 外部仍按 5ms 调用；内部会按滚动窗口更新速度估计 */
|
||||
void Motor_Update_RPM(float dt_s);
|
||||
// 获取计算好的实时转速 (正数代表车轮正使车体【向前】行驶)
|
||||
float Get_Motor_RPM(Motor_ID_t id);
|
||||
// 获取自上次调用以来的累加脉冲增量，并自动清零
|
||||
void Motor_Get_And_Clear_Delta_Ticks(int16_t* d_fl, int16_t* d_rl, int16_t* d_fr, int16_t* d_rr);
|
||||
|
||||
#endif /* __MOTOR_H */
|
||||
#endif /* __MOTOR_H */
|
||||
|
||||
@@ -58,6 +58,7 @@ void SysTick_Handler(void);
|
||||
void CAN1_RX0_IRQHandler(void);
|
||||
void CAN1_SCE_IRQHandler(void);
|
||||
void TIM6_DAC_IRQHandler(void);
|
||||
void TIM7_IRQHandler(void);
|
||||
void OTG_FS_IRQHandler(void);
|
||||
/* USER CODE BEGIN EFP */
|
||||
|
||||
|
||||
@@ -44,6 +44,8 @@ extern TIM_HandleTypeDef htim5;
|
||||
|
||||
extern TIM_HandleTypeDef htim6;
|
||||
|
||||
extern TIM_HandleTypeDef htim7;
|
||||
|
||||
extern TIM_HandleTypeDef htim8;
|
||||
|
||||
extern TIM_HandleTypeDef htim9;
|
||||
@@ -58,6 +60,7 @@ void MX_TIM3_Init(void);
|
||||
void MX_TIM4_Init(void);
|
||||
void MX_TIM5_Init(void);
|
||||
void MX_TIM6_Init(void);
|
||||
void MX_TIM7_Init(void);
|
||||
void MX_TIM8_Init(void);
|
||||
void MX_TIM9_Init(void);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user