繼卡爾曼封裝之后推出第二個封裝，PID算法，這個其實比上個簡單多了下面貼出代碼
/**
??******************************************************************************
??* @file ???PID_Control.h
??* @author ?willieon
??* @version V0.1
??* @date ???January-2015
??* @brief ??PID控制算法頭文件
??* ???????????????????????定義結(jié)構(gòu)體類型以及聲明函數(shù)
??* ???????????????????????#define IF_THE_INTEGRAL_SEPARATION ?0/1 ?為積分分離標(biāo)志
??******************************************************************************
??**/
?
#ifndef __PID_CONTROL_H__
#define __PID_CONTROL_H__
?
#define IF_THE_INTEGRAL_SEPARATION ?0 ???
//#define IF_THE_INTEGRAL_SEPARATION ?1 ??//是否積分分離 ?0-不分離，1 -分離
?
typedef struct
{
????????double SetPoint; // 設(shè)定目標(biāo) Desired Value ??
????????double Proportion; // 比例常數(shù) Proportional Const
????????double Integral; // 積分常數(shù) Integral Const
????????double Derivative; // 微分常數(shù) Derivative Const ??
????????double LastError; // Error[-1]
????????double PrevError; // Error[-2]
????????double SumError; // Sums of Errors ?
}PID;
?
#if IF_THE_INTEGRAL_SEPARATION ???????????//是否積分分離預(yù)編譯開始
?
double PIDCalc(double NextPoint ,double SepLimit, PID *pp); ??//帶積分分離的PID運算
?
#else
?
double PIDCalc( double NextPoint, PID *pp); ????//不帶積分分離的PID運算
?
#endif ???????//是否積分分離預(yù)編譯結(jié)束
?
void PIDInit (double SetPoint, double Proportion, double Integral, double Derivative, PID *pp);
?
#endif
/**
??******************************************************************************
??* @file ???PID_Control.c
??* @author ?willieon
??* @version V0.1
??* @date ???January-2015
??* @brief ??PID控制算法函數(shù)代碼
??* ???????
??*
??******************************************************************************
??**/
?
#include "PID_Control.h"
#include "math.h"
?
/*************************************************************************************
* ???????名 ???稱： double PIDCalc( PID *pp, double NextPoint ,double SepLimit)
* ???????功 ???能： PID控制運算
* ???????入口參數(shù)： PID *pp ?- 定義的運算所需變量的結(jié)構(gòu)體
* ??????????????????????????NextPoint - 負反饋輸入值
* ??????????????????????????SepLimit ?- 積分分離上限
* ???????出口參數(shù)： 返回PID控制量
* ???????說 ???明： 默認不進行積分分離，如果用戶需要使用積分分離，需在PID_Control.h中
* ???????????????????????????????將 #define IF_THE_INTEGRAL_SEPARATION ?0 ?改為
* ???????????????????????????#define IF_THE_INTEGRAL_SEPARATION ?1
* ???????調(diào)用方法： 進行積分分離時入口參數(shù)為3個，具體方法如下：
* ???????????????????????????????PID PIDControlStruct ; ??//定義PID運算結(jié)構(gòu)體
* ???????????????????????????????PIDInit(50, 0.24, 0.04, 0.2, &PIDControlStruct);//結(jié)構(gòu)體初始化，注意&符號不能省
* ???????????????????????????????ControlData = PIDCalc(ReadData, 200, &PIDControlStruct); ??//控制量 = PIDCalc(反饋值,積分分離上限,PID運算結(jié)構(gòu)體)
*
***************************************************************************************
*/
?
#if IF_THE_INTEGRAL_SEPARATION
?
double PIDCalc(double NextPoint ,double SepLimit, PID *pp)
{
????????double dError, Error,Flag; ??
????????Error = pp->SetPoint - NextPoint; ????????// 偏差
????????if(abs(Error) > SepLimit) ???????//當(dāng)偏差大于分離上限積分分離
????????{
????????????????Flag = 0;
????????}
????????else ??????//當(dāng)偏差小于分離上限，積分項不分離
????????{
????????????????Flag = 1;
????????????????pp->SumError += Error; ????????// 積分 ?
????????}
????????dError = pp->LastError - pp->PrevError; ????????// 當(dāng)前微分
????????pp->PrevError = pp->LastError;
????????pp->LastError = Error; ?
????????return (
????????????????pp->Proportion ???????????????* ???????????????Error ????????????????// 比例項
????????????????+ Flag * pp->Integral ???????* ???????????????pp->SumError ????????// 積分項
????????????????+ pp->Derivative ???????????????* ???????????????dError ????????????????// 微分項
????????????????);
}
?
#else
?
double PIDCalc( double NextPoint, PID *pp)
{ ?
????????double dError, Error; ??
????????Error = pp->SetPoint - NextPoint; ????????????????????????// 偏差
????????pp->SumError += Error; ???????????????????????????????????????// 積分 ?
????????dError = pp->LastError - pp->PrevError; ???????????????// 當(dāng)前微分
????????pp->PrevError = pp->LastError;
????????pp->LastError = Error; ?
????????return (pp->Proportion ???????* ???????Error ???????????????// 比例項
????????????????+ pp->Integral ???????????????* ???????pp->SumError ????????// 積分項
????????????????+ pp->Derivative ???????* ???????dError ????????// 微分項
????????????????);
}
?
#endif
?
?
/*************************************************************************************
* ???????名 ???稱： double PIDCalc( PID *pp, double NextPoint ,double SepLimit)
* ???????功 ???能： PID初始化設(shè)定
* ???????入口參數(shù)： PID *pp ?- 定義的運算所需變量的結(jié)構(gòu)體
* ??????????????????????????SetPoint - 設(shè)定的目標(biāo)值
* ??????????????????????????Proportion，Integral ，Derivative - P,I,D系數(shù)
* ???????出口參數(shù)： 無
* ???????說 ???明： ???????
* ???????調(diào)用方法： ?PID PIDControlStruct ; ??//定義PID運算結(jié)構(gòu)體
* ???????????????????????????????PIDInit(50, 0.24, 0.04, 0.2, &PIDControlStruct);//結(jié)構(gòu)體初始化，注意&符號不能省
* ???????????????????????????????因為函數(shù)需要傳入一個指針，需要對結(jié)構(gòu)體取首地址傳給指針
*
***************************************************************************************
*/
?
?
void PIDInit (double SetPoint, double Proportion, double Integral, double Derivative, PID *pp)
{ ?
????????pp -> SetPoint = SetPoint; // 設(shè)定目標(biāo) Desired Value ??
????????pp -> Proportion = Proportion; // 比例常數(shù) Proportional Const
????????pp -> Integral = Integral; // 積分常數(shù) Integral Const
????????pp -> Derivative = Derivative; // 微分常數(shù) Derivative Const ??
????????pp -> LastError = 0; // Error[-1]
????????pp -> PrevError = 0; // Error[-2]
????????pp -> SumError = 0; // Sums of Errors
?
????????//memset ( pp,0,sizeof(struct PID)); ??//need include "string.h"
}
　　將前篇 卡爾曼 和本次的 PID放在一起，在VS2010平臺中調(diào)試 代碼如下
#include "kalman.h"
?
#include "stdio.h"
#include "stdlib.h"
#include "PID_Control.h"
?
void main(void)
?
{
????????KalmanCountData k;
????????PID PIDControlStruct;
????????Kalman_Filter_Init(&k);
????????PIDInit(50, 1, 0.04, 0.2, &PIDControlStruct);
????????int m,n;
?
????????double out;
?
????????for(int a = 0;a<80;a++)
????????{
????????????????m = 1+ rand() %100;
????????????????n = 1+ rand() %100;
????????????????Kalman_Filter((float)m,(float)n,&k);
????????????????out = PIDCalc(k.Angle_Final, &PIDControlStruct);
????????????????printf("%3d and %3d is %6f -pid- %6f\r\n",m,n,k.Angle_Final,out);
?
????????}
}
?