mirror of
https://gitee.com/dlmu-cone/bf_original_balance_chassis
synced 2026-07-24 03:27:45 +08:00
finish all bsp utilities
This commit is contained in:
NeoZng
@@ -1,37 +1,38 @@
|
||||
|
||||
/**
|
||||
* @file controller.c
|
||||
* @author wanghongxi
|
||||
* @author modified by neozng
|
||||
* @brief PID控制器定义
|
||||
* @version beta
|
||||
* @date 2022-11-01
|
||||
*
|
||||
* @copyrightCopyright (c) 2022 HNU YueLu EC all rights reserved
|
||||
*/
|
||||
#include "controller.h"
|
||||
#include <memory.h>
|
||||
|
||||
/******************************* PID CONTROL *********************************/
|
||||
// PID优化环节函数声明
|
||||
static void f_Trapezoid_Intergral(PID_t *pid);
|
||||
static void f_Integral_Limit(PID_t *pid);
|
||||
static void f_Derivative_On_Measurement(PID_t *pid);
|
||||
static void f_Changing_Integration_Rate(PID_t *pid);
|
||||
static void f_Output_Filter(PID_t *pid);
|
||||
static void f_Derivative_Filter(PID_t *pid);
|
||||
static void f_Output_Limit(PID_t *pid);
|
||||
static void f_Proportion_Limit(PID_t *pid);
|
||||
static void f_PID_ErrorHandle(PID_t *pid);
|
||||
static void f_Trapezoid_Intergral(PID_t *pid); // 梯形积分
|
||||
static void f_Integral_Limit(PID_t *pid); // 积分限幅
|
||||
static void f_Derivative_On_Measurement(PID_t *pid); // 微分先行(仅使用反馈微分)
|
||||
static void f_Changing_Integration_Rate(PID_t *pid); // 变速积分
|
||||
static void f_Output_Filter(PID_t *pid); // 输出滤波
|
||||
static void f_Derivative_Filter(PID_t *pid); // 微分滤波(采集时)
|
||||
static void f_Output_Limit(PID_t *pid); // 输出限幅
|
||||
static void f_PID_ErrorHandle(PID_t *pid); // 堵转保护
|
||||
|
||||
/**
|
||||
* @brief
|
||||
* @brief 初始化PID,设置参数和启用的优化环节,将其他数据置零
|
||||
*
|
||||
* @param pid
|
||||
* @param config
|
||||
* @param pid PID实例
|
||||
* @param config PID初始化设置
|
||||
*/
|
||||
void PID_Init(PID_t* pid,PID_Init_config_s *config)
|
||||
void PID_Init(PID_t *pid, PID_Init_config_s *config)
|
||||
{
|
||||
// utilize the quality of struct that its memeory is continuous
|
||||
memcpy(pid, config, sizeof(PID_Init_config_s));
|
||||
memset(&pid->Measure,0,sizeof(PID_t)-sizeof(PID_Init_config_s));
|
||||
|
||||
// // DWT定时器计数变量清零
|
||||
// // reset DWT Timer count counter
|
||||
// pid->DWT_CNT = 0;
|
||||
|
||||
// // 设置PID异常处理 目前仅包含电机堵转保护
|
||||
// pid->ERRORHandler.ERRORCount = 0;
|
||||
// pid->ERRORHandler.ERRORType = PID_ERROR_NONE;
|
||||
// set rest of memory to 0
|
||||
memset(&pid->Measure, 0, sizeof(PID_t) - sizeof(PID_Init_config_s));
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -1,19 +1,18 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file controller.h
|
||||
* @author Wang Hongxi
|
||||
* @version V1.1.3
|
||||
* @date 2021/7/3
|
||||
* @brief
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
******************************************************************************
|
||||
* @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 "string.h"
|
||||
@@ -26,36 +25,37 @@
|
||||
#define abs(x) ((x > 0) ? x : -x)
|
||||
#endif
|
||||
|
||||
|
||||
/******************************* PID CONTROL *********************************/
|
||||
// PID 优化环节使能标志位
|
||||
typedef enum pid_Improvement_e
|
||||
{
|
||||
NONE = 0X00, //0000 0000
|
||||
Integral_Limit = 0x01, //0000 0001
|
||||
Derivative_On_Measurement = 0x02, //0000 0010
|
||||
Trapezoid_Intergral = 0x04, //0000 0100
|
||||
Proportional_On_Measurement = 0x08, //0000 1000
|
||||
OutputFilter = 0x10, //0001 0000
|
||||
ChangingIntegrationRate = 0x20, //0010 0000
|
||||
DerivativeFilter = 0x40, //0100 0000
|
||||
ErrorHandle = 0x80, //1000 0000
|
||||
NONE = 0b00000000, // 0000 0000
|
||||
Integral_Limit = 0b00000001, // 0000 0001
|
||||
Derivative_On_Measurement = 0b00000010, // 0000 0010
|
||||
Trapezoid_Intergral = 0b00000100, // 0000 0100
|
||||
Proportional_On_Measurement = 0b00001000, // 0000 1000
|
||||
OutputFilter = 0b00010000, // 0001 0000
|
||||
ChangingIntegrationRate = 0b00100000, // 0010 0000
|
||||
DerivativeFilter = 0b01000000, // 0100 0000
|
||||
ErrorHandle = 0b10000000, // 1000 0000
|
||||
} PID_Improvement_e;
|
||||
|
||||
/* PID 报错类型枚举*/
|
||||
typedef enum errorType_e
|
||||
{
|
||||
PID_ERROR_NONE = 0x00U,
|
||||
Motor_Blocked = 0x01U
|
||||
} ErrorType_e;
|
||||
|
||||
typedef struct
|
||||
typedef struct
|
||||
{
|
||||
uint64_t ERRORCount;
|
||||
ErrorType_e ERRORType;
|
||||
} PID_ErrorHandler_t;
|
||||
|
||||
typedef struct
|
||||
/* PID结构体 */
|
||||
typedef struct
|
||||
{
|
||||
//---------------------------------- init config block
|
||||
//---------------------------------- init config block
|
||||
// config parameter
|
||||
float Kp;
|
||||
float Ki;
|
||||
@@ -64,13 +64,13 @@ typedef struct
|
||||
float MaxOut;
|
||||
float IntegralLimit;
|
||||
float DeadBand;
|
||||
float CoefA; //For Changing Integral
|
||||
float CoefB; //ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
|
||||
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;
|
||||
|
||||
uint8_t Improve;
|
||||
//-----------------------------------
|
||||
//-----------------------------------
|
||||
// for calculating
|
||||
float Measure;
|
||||
float Last_Measure;
|
||||
@@ -96,7 +96,7 @@ typedef struct
|
||||
} PID_t;
|
||||
|
||||
/* 用于PID初始化的结构体*/
|
||||
typedef struct
|
||||
typedef struct
|
||||
{
|
||||
// config parameter
|
||||
float Kp;
|
||||
@@ -106,8 +106,8 @@ typedef struct
|
||||
float MaxOut;
|
||||
float IntegralLimit;
|
||||
float DeadBand;
|
||||
float CoefA; //For Changing Integral
|
||||
float CoefB; //ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
|
||||
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;
|
||||
|
||||
@@ -115,32 +115,22 @@ typedef struct
|
||||
|
||||
} PID_Init_config_s;
|
||||
|
||||
/**
|
||||
* @brief 初始化PID实例
|
||||
*
|
||||
* @param pid PID实例指针
|
||||
* @param config PID初始化配置
|
||||
*/
|
||||
void PID_Init(PID_t *pid, PID_Init_config_s *config);
|
||||
|
||||
|
||||
void PID_Init(PID_t* pid,PID_Init_config_s* config);
|
||||
/**
|
||||
* @brief 计算PID输出
|
||||
*
|
||||
* @param pid PID实例指针
|
||||
* @param measure 反馈值
|
||||
* @param ref 设定值
|
||||
* @return float PID计算输出
|
||||
*/
|
||||
float PID_Calculate(PID_t *pid, float measure, float ref);
|
||||
|
||||
|
||||
/*************************** Tracking Differentiator ***************************/
|
||||
typedef struct
|
||||
{
|
||||
float Input;
|
||||
|
||||
float h0;
|
||||
float r;
|
||||
|
||||
float x;
|
||||
float dx;
|
||||
float ddx;
|
||||
|
||||
float last_dx;
|
||||
float last_ddx;
|
||||
|
||||
uint32_t DWT_CNT;
|
||||
float dt;
|
||||
} TD_t;
|
||||
|
||||
void TD_Init(TD_t *td, float r, float h0);
|
||||
float TD_Calculate(TD_t *td, float input);
|
||||
|
||||
#endif
|
||||
#endif
|
||||
Reference in New Issue
Block a user