mirror of
https://gitee.com/dlmu-cone/tronone-h7-scaffold
synced 2026-07-24 03:27:45 +08:00
152 lines
3.9 KiB
C
152 lines
3.9 KiB
C
/**
|
|
******************************************************************************
|
|
* @file controller.h
|
|
* @author Wang Hongxi
|
|
* @version V1.1.3
|
|
* @date 2021/7/3
|
|
* @brief
|
|
******************************************************************************
|
|
* @attention
|
|
*
|
|
******************************************************************************
|
|
*/
|
|
#ifndef _CONTROLLER_H
|
|
#define _CONTROLLER_H
|
|
|
|
#include "main.h"
|
|
#include "stdint.h"
|
|
#include "memory.h"
|
|
#include "stdlib.h"
|
|
#include "bsp_dwt.h"
|
|
#include "arm_math.h"
|
|
#include <math.h>
|
|
|
|
#ifndef abs
|
|
#define abs(x) ((x > 0) ? x : -x)
|
|
#endif
|
|
|
|
// PID 优化环节使能标志位
|
|
typedef enum
|
|
{
|
|
PID_IMPROVE_NONE = 0b000000000, // 无优化
|
|
PID_Integral_Limit = 0b000000001, // 积分限幅
|
|
PID_Derivative_On_Measurement = 0b000000010, // 微分先行
|
|
PID_Trapezoid_Intergral = 0b000000100, // 梯形积分
|
|
PID_Proportional_On_Measurement = 0b000001000, // 比例先行
|
|
PID_OutputFilter = 0b000010000, // 输出滤波
|
|
PID_ChangingIntegrationRate = 0b000100000, // 变速积分
|
|
PID_DerivativeFilter = 0b001000000, // 微分滤波
|
|
PID_ErrorHandle = 0b010000000, // 错误处理
|
|
PID_FeedForward = 0b100000000, // 前馈控制
|
|
} PID_Improvement_e;
|
|
|
|
/* PID 报错类型枚举*/
|
|
typedef enum errorType_e
|
|
{
|
|
PID_ERROR_NONE = 0x00U,
|
|
PID_MOTOR_BLOCKED_ERROR = 0x01U
|
|
} ErrorType_e;
|
|
|
|
typedef struct
|
|
{
|
|
uint64_t ERRORCount;
|
|
ErrorType_e ERRORType;
|
|
} PID_ErrorHandler_t;
|
|
|
|
/* PID结构体 */
|
|
typedef struct
|
|
{
|
|
//---------------------------------- init config block
|
|
// config parameter
|
|
float Kp;
|
|
float Ki;
|
|
float Kd;
|
|
float MaxOut;
|
|
float DeadBand;
|
|
|
|
// improve parameter
|
|
PID_Improvement_e Improve;
|
|
float IntegralLimit; // 积分限幅
|
|
float CoefA; // 变速积分 For Changing Integral
|
|
float CoefB; // 变速积分 ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
|
|
float Output_LPF_RC; // 输出滤波器 RC = 1/omegac
|
|
float Derivative_LPF_RC; // 微分滤波器系数
|
|
|
|
//-----------------------------------
|
|
// Feed Forward Control (FFC) parameters
|
|
float FFC_Kp; // 前馈比例系数
|
|
float FFC_Kd; // 前馈微分系数
|
|
float FFC_Ka; // 前馈加速度系数
|
|
float FFC_LPF_RC; // 前馈低通滤波器系数
|
|
float FFC_Output; // 前馈输出值
|
|
float FFC_Set_History[3]; // 前馈设定值历史 [NOW, LAST, LLAST]
|
|
|
|
//-----------------------------------
|
|
// for calculating
|
|
float Measure;
|
|
float Last_Measure;
|
|
float Err;
|
|
float Last_Err;
|
|
float Last_ITerm;
|
|
|
|
float Pout;
|
|
float Iout;
|
|
float Dout;
|
|
float ITerm;
|
|
|
|
float Output;
|
|
float Last_Output;
|
|
float Last_Dout;
|
|
|
|
float Ref;
|
|
|
|
uint32_t DWT_CNT;
|
|
float dt;
|
|
|
|
PID_ErrorHandler_t ERRORHandler;
|
|
} PIDInstance;
|
|
|
|
/* 用于PID初始化的结构体*/
|
|
typedef struct // config parameter
|
|
{
|
|
// basic parameter
|
|
float Kp;
|
|
float Ki;
|
|
float Kd;
|
|
float MaxOut; // 输出限幅
|
|
float DeadBand; // 死区
|
|
|
|
// improve parameter
|
|
PID_Improvement_e Improve;
|
|
float IntegralLimit; // 积分限幅
|
|
float CoefA; // AB为变速积分参数,变速积分实际上就引入了积分分离
|
|
float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
|
|
float Output_LPF_RC; // RC = 1/omegac
|
|
float Derivative_LPF_RC;
|
|
|
|
// Feed Forward Control (FFC) parameters
|
|
float FFC_Kp; // 前馈比例系数
|
|
float FFC_Kd; // 前馈微分系数
|
|
float FFC_Ka; // 前馈加速度系数
|
|
float FFC_LPF_RC; // 前馈低通滤波器系数
|
|
} PID_Init_Config_s;
|
|
|
|
/**
|
|
* @brief 初始化PID实例
|
|
* @todo 待修改为统一的PIDRegister风格
|
|
* @param pid PID实例指针
|
|
* @param config PID初始化配置
|
|
*/
|
|
void PIDInit(PIDInstance *pid, PID_Init_Config_s *config);
|
|
|
|
/**
|
|
* @brief 计算PID输出
|
|
*
|
|
* @param pid PID实例指针
|
|
* @param measure 反馈值
|
|
* @param ref 设定值
|
|
* @return float PID计算输出
|
|
*/
|
|
float PIDCalculate(PIDInstance *pid, float measure, float ref);
|
|
|
|
#endif |