?? pid.cpp
字號:
// PID.cpp: implementation of the CPID class.
//
//////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include "FollowLine.h"
#include "PID.h"
#include "math.h"
#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
CPID::CPID()
{
ThetaExp = 0;
ThetaFB = 0;
Kd = 0.0f;
Ki = 0.0f;
Kp = 0.0f;
LastError = 0;
integral = 0.0f;
}
CPID::~CPID()
{
}
//參數調節
void CPID::pid_tune(float m_Kp,float m_Ki,float m_Kd, int m_DeadBand)
{
Kp = m_Kp;
Ki = m_Ki;
Kd = m_Kd;
DeadBand = m_DeadBand;
}
//pid計算
float CPID::pid_calc(int g_phei)
{
int err;
float pterm,dterm,result,ferror;
ThetaFB = g_phei;
err = ThetaExp - ThetaFB;
if (abs(err)>DeadBand)
{
ferror = (float)err;
pterm = Kp*ferror;
// pid進行限幅
if (fabs(pterm)>90.0)
{
integral = 0.0f;
}
else
{
integral += Ki*ferror;
if (integral>90.0)
{
integral = 90.0;
}
else if (integral<-90.0)
{
integral = -90.0;
}
// else if (integral<0.0)
// {
// integral = 0.0;
// }
}
dterm = ((float)(err - LastError))*Kd;
result = pterm + integral + dterm;
}
else result = integral;
//保存上一時刻的誤差值
LastError = err;
return (-result);
}
// 清楚上一時刻的歷史值
void CPID::ClearHistory()
{
integral = 0.0f;
LastError = 0;
}
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -