/*============================================
File Name  : pid.cpp
Description: pid controller
Version	   : 0.1 that is debug version
Author     : he hai long
Date       : 10/22/2003
==============================================*/

#include "pid.h"

void PidInit(struct PidData *data)
{
	data->States.P = 0;
	data->States.I = 0;
	data->States.D = 0;
	data->States.yold = 0;
	data->Par.K = 4.4;
	data->Par.Ti = 0.4;
	data->Par.Td = 0.2;
	data->Par.Tt = 10;
	data->Par.N = 10;
	data->Par.b = 1;
	data->Par.ulow = -1;
	data->Par.uhigh = 1;
	data->Par.h = 0.03;
	data->Par.bi = data->Par.K*data->Par.h/data->Par.Ti;
	data->Par.ar = data->Par.h/data->Par.Tt;
	data->Par.bd = data->Par.K*data->Par.N*data->Par.Td/(data->Par.Td+data->Par.N*data->Par.h);
	data->Par.ad = data->Par.Td/(data->Par.Td+data->Par.N*data->Par.h);
}

void PidCalculateOutput(struct PidData *data)
{
	//Proportional part
	data->States.P = data->Par.K*(data->Par.b*data->Signals.uc-data->Signals.y);
	//Derivative part
	data->States.D = data->Par.ad*data->States.D
		-data->Par.bd*(data->Signals.y-data->States.yold);
	//calculate control signal
	data->Signals.v = data->States.P+data->States.I+data->States.D;
	//handle actuator limitations
	if(data->Signals.v < data->Par.ulow)
	{
		data->Signals.u = data->Par.ulow;
	}
	else if(data->Signals.v > data->Par.uhigh)
	{
		data->Signals.u = data->Par.uhigh;
	}
	else
	{
		data->Signals.u = data->Signals.v;
	}
}
void PidUpdateStates(struct PidData *data)
{
	//integral part
	data->States.I = data->States.I+
		data->Par.bi*(data->Signals.uc-data->Signals.y)+
		data->Par.ar*(data->Signals.u-data->Signals.v);
	data->States.yold = data->Signals.y;
}


void PidReset(struct PidData *data)
{
	data->States.P = 0;
	data->States.I = 0;
	data->States.D = 0;
	data->States.yold = 0;
}

/*============================================
	no more
=============================================*/