69 lines
1.5 KiB
C
69 lines
1.5 KiB
C
#ifndef __LADRC_H
|
|
#define __LADRC_H
|
|
|
|
#include <stdint.h>
|
|
#include "motor.h"
|
|
|
|
typedef struct {
|
|
/* 外部命令与内部参考 */
|
|
float v_cmd; /* 上层下发的目标 RPM */
|
|
float v; /* 内部斜坡后的参考 RPM */
|
|
float y; /* 实际速度 RPM */
|
|
|
|
/* LESO 状态 */
|
|
float z1;
|
|
float z2;
|
|
|
|
/* 核心参数 */
|
|
float wc;
|
|
float wo;
|
|
float b0;
|
|
|
|
/* LESO 增益 */
|
|
float beta1;
|
|
float beta2;
|
|
|
|
/* 采样与限幅 */
|
|
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;
|
|
|
|
} 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);
|
|
|
|
extern LADRC_TypeDef ladrc_motors[4];
|
|
|
|
void FourWheel_LADRC_Init(void);
|
|
void FourWheel_Set_Target_RPM(float fl_rpm, float rl_rpm, float fr_rpm, float rr_rpm);
|
|
void FourWheel_LADRC_ResetAll(void);
|
|
void FourWheel_LADRC_Control_Loop(void);
|
|
|
|
/* 命令目标与内部参考分开暴露，便于调试 */
|
|
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 */
|