// 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;
}