#include "pid.h"
#include "board.h"
#include <rtthread.h>
//https://blog.csdn.net/weibo1230123/article/details/80812211
struct _pid sPID;
/*---------------------------------------------------------------------
FuncName: PID_init
Describe:
Input :
output :
Other :
---------------------------------------------------------------------*/
void PID_init(void)
{
printf("PID_init begin \n");
sPID.SetPoint = 100.0;
sPID.ActualPoint=0.0;
sPID.LastError=0.0;
sPID.PrevError=0.0;
sPID.Proportion=0.2;
sPID.Integral=0.015;
sPID.Derivative=0.2;
printf("PID_init end \n");
}
/*---------------------------------------------------------------------
FuncName: PIDSetPoint
Describe: 设置 PID 调节的目标值
Input : 期望值
output : 无
Other :
---------------------------------------------------------------------*/
void PIDSetPoint(struct _pid *sptr,double SetParam)
{
sptr->SetPoint = SetParam;
}
/*---------------------------------------------------------------------
FuncName: PIDGetSetpoint(struct _pid *sptr)
Describe: 读取 PID 调节设置的目标值
Input : 无
output : 所设置的期望值
Other :
---------------------------------------------------------------------*/
double PIDGetSetpoint(struct _pid *sptr)
{
return sptr->SetPoint;
}
/*---------------------------------------------------------------------
FuncName: PIDSetKp
Describe: 设置 PID 的 Kp 值
Input : Kp 数值
output : 无
Other :
---------------------------------------------------------------------*/
void PIDSetKp(struct _pid *sptr,double dKpp)
{
sptr->Proportion = dKpp;
}
/*---------------------------------------------------------------------
FuncName: PIDGetKp
Describe: 读取 PID 中所设置的 Kp 值
Input : 无
output : Kp 数值
Other :
---------------------------------------------------------------------*/
double PIDGetKp(struct _pid *sptr)
{
return sptr->Proportion;
}
/*---------------------------------------------------------------------
FuncName: PIDSetKi
Describe: 设置 PID 的 Ki 值
Input : Ki 数值
output : 无
Other :
---------------------------------------------------------------------*/
void PIDSetKi(struct _pid *sptr,double dKii)
{
sptr->Integral = dKii;
}
/*---------------------------------------------------------------------
FuncName: double PIDGetKi(void)
Describe: 读取 PID 中所设置的 Ki 值
Input : 无
output : Ki 数值
Other :
---------------------------------------------------------------------*/
double PIDGetKi(struct _pid *sptr)
{
return sptr->Integral;
}
/*---------------------------------------------------------------------
FuncName: PIDSetKd
Describe: 设置 PID 的 Kd 值
Input : Kd 数值
output : 无
Other :
---------------------------------------------------------------------*/
void PIDSetKd(struct _pid *sptr,double dKdd)
{
sptr->Derivative = dKdd;
}
/*---------------------------------------------------------------------
FuncName: PIDGetKd
Describe: 读取 PID 中所设置的 Kd 值
Input :
output : Kd 数值
Other :
---------------------------------------------------------------------*/
double PIDGetKd(struct _pid *sptr)
{
return sptr->Derivative;
}
//增量式 PID 控制设计
/*---------------------------------------------------------------------
FuncName: IncPIDCalc
Describe: 增量PID设计
Input : 下一个值
output : 返回增量
Other :
---------------------------------------------------------------------*/
double IncPIDCalc(struct _pid *sptr,double NextPoint)
{
double iError, iIncpid;
//当前误差
iError = sptr->SetPoint - NextPoint;
//增量计算
iIncpid = sptr->Proportion * iError //E[k]项
- sptr->Integral * sptr->LastError //E[k-1]项
+ sptr->Derivative * sptr->PrevError; //E[k-2]项
//存储误差,用于下次计算
sptr->PrevError = sptr->LastError;
sptr->LastError = iError;
//返回增量值
return(iIncpid);
}
float IncVal = 0;
void test_pid(void)
{
//下一个点等于 当前值+增量
IncVal = IncPIDCalc(&sPID,sPID.ActualPoint);
printf("Inc:%f %f\r\n",IncVal,sPID.ActualPoint);
rt_thread_delay(100);
}
//sPID.ActualPoint += IncVal;//当前值加上增量
//printf("TTTTT:%f \r\n",sPID.ActualPoint);