代码规范

User_Code/module/algorithm/controller/pid/pid.h

@@ -0,0 +1,136 @@
+/**
+ ******************************************************************************
+ * @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 = 0b00000000, // 无优化
+    PID_Integral_Limit = 0b00000001, // 积分限幅
+    PID_Derivative_On_Measurement = 0b00000010, // 微分先行
+    PID_Trapezoid_Intergral = 0b00000100, // 梯形积分
+    PID_Proportional_On_Measurement = 0b00001000, // 比例先行
+    PID_OutputFilter = 0b00010000, // 输出滤波
+    PID_ChangingIntegrationRate = 0b00100000, // 变积分
+    PID_DerivativeFilter = 0b01000000, // 微分滤波
+    PID_ErrorHandle = 0b10000000, // 误差处理
+} 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; // 微分滤波器系数
+
+    //-----------------------------------
+    // 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;
+} 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