/******************************************************************************
PID Teszt
*******************************************************************************/

//================================
// SYSTEM   
#include <htc.h>
#include <math.h>

/***************************************************************************
    PID Parameters
***************************************************************************/

#define PID_MAX			100		// pid max %
#define PID_MIN 		  0		// pid min %
#define MAX_P			1000
#define MAX_I			1000
#define MAX_D			1000



// PID parameters
double pid_p = 92.4; 		// P param
double pid_i = 230; 		// I param
double pid_d = 57.5; 		// D param

double s_point = 65;		// Set point
double m_point = 64.8;		// Measured point
double pid;					// Current PID

// pid_tak
double sample_sec = 1; 		// Sample
double k0;					// k0 value for PID controller
double k1;					// k1 value for PID controller

double m_point_1 = 64.7;	// PV[k-1]
double m_point_2 = 64.6;	// PV[k-2]

// pid_book
double err_1 = 0.1;			//	Error[-1] - Prev error
double pi_1 = 230;			//	Prev I	

// pid_young
double err_2 = 0.1;			//	Error[-2]
double err_sum = 0.2;		//	Error[-2]


// pid_hc
#define LOW_BAND		10		// Arround s_point
#define HIGH_BAND		10
#define I_COUNT_MAX		2   	// intervals before increasing I
#define I_COUNT_DEC		4       // times longer to wait b4 decrementing
#define I_VALUE_MAX		1000    // Max I value - Limits overshoot
char ct_i = 0;					// I counter
double	calc_i;					// Calc I


//===================================================================
//	Takahashi PID controller
void pid_tak_init( void )
{
	if( pid_i == 0 )
	{
		k0 = 0;
	}
	else
	{
		k0 = pid_p * sample_sec / pid_i;
	}
	k1 = pid_p * pid_d / sample_sec;
}


void pid_tak_calc( void )
{
	double err, pp, pi, pd;

	// Error
	err = s_point - m_point; 								// e[k] = SP[k] - PV[k]
	
	// P
	pp = pid_p * ( m_point_1 - m_point );					// pid_p*(PV[k-1] - PV[k])
	
	// I
	pi = k0 * err; 											// pid_p*sample_sec/pid_i * e[k]
	
	// D
	pd = k1 * ( 2.0 * m_point_1 - m_point - m_point_2 );	// pid_d/sample_sec*(2*PV[k-1] - PV[k] - PV[k-2]))
	
	// PID
	pid += pp + pi + pd;

	if( pid > PID_MAX )
	{
		pid = PID_MAX;
	}
	else if( pid < PID_MIN )
	{
		pid = PID_MIN;
	}
	
	// Save
	m_point_2 = m_point_1;		// PV[k-2] = PV[k-1]
	m_point_1 = m_point;		// PV[k-1] = PV[k]
}


//===================================================================
//	Book : Microcontroller-based Temperature Monitoring and Control
void pid_book_calc( void )
{
	double err, pi, pd;
	
	// Error
	err = s_point - m_point;
	
	// I
	pi = pid_i * err + pi_1;
	
	// D
	pd = pid_d *( err - err_1 );
	
	// PID
	pid = pi + pid_p * err + pd;
	
	if( pid  > PID_MAX )
	{
		pi = pi_1;
		pid = PID_MAX;
	}
	else if( pid < PID_MIN )
	{
		pi = pi_1;
		pid = PID_MIN;
	}
	
	// Save
	pi_1 = pi;
	err_1 = err;
}


//===================================================================
//	Greg Young Z-World PID controller
void pid_young_calc( void )
{
	double err, err_d, pp, pi, pd;

	// Error
	err = s_point;
	err_sum += err - m_point;
	err_d = err_1-err_2;
	
	// P
	pp = pid_p * err;
		
	// I
	pi = pid_p * err_sum;
			
	// D
	pd = pid_d * err_d;
	
	// PID
	pid = pp + pi + pd;
	
	if( pid > PID_MAX )
	{
		pid = PID_MAX;
	}
	else if( pid < PID_MIN )
	{
		pid = PID_MIN;
	}
	
	// Save
	err_2 = err_1;
	err_1 = err;
}


//===================================================================
// PID_SIM - PID szabályozó szimulátor program
void pid_sim_calc( void )
{
	double err, pp, pi, pd;

	// Error
	err = s_point - m_point;

	// P
	pp = pid_p * err;
	if ( pp > MAX_P )
		pp = MAX_P;
	else if( pp < ( -1 * MAX_P ) ) 
		pp = -1 * MAX_P;
		
	// I
	pi += pid_i * err;
	if( pi > MAX_I )
		pi = MAX_I;
	else if( pi < ( -1 * MAX_I ) )
		pi = -1 * MAX_I;
		
	// D
	pd = pid_d * (err - err_1);
	if( pd > MAX_D )
		pd = MAX_D;
	else if( pd < ( -1 * MAX_D ) )
		pd = -1 * MAX_D;

	// PID
	pid = pp + pi + pd;
	
	if( pid > PID_MAX )
	{
		pid = PID_MAX;
	}
	else if( pid < PID_MIN )
	{
		pid = PID_MIN;
	}
	
	// Save
	err_2 = err_1;
	err_1 = err;
}


//===================================================================
// PIC12c6xx Based Heater Controller
void pid_hc_calc( void )
{
	double err;
	
	// Errror
	err = s_point - m_point; 
 
 
 if( err >= 0 )
 {
	// Heat
	if( m_point < ( s_point - LOW_BAND ) )
	{
		pid = 100;		// Under BAND - full heating
		calc_i = 0;
	}
	else
	{
		if( err )
		{
			if( ct_i++ > I_COUNT_MAX )
			{
				 ct_i = 0;
				 calc_i++;
				 if( calc_i > I_VALUE_MAX )
					calc_i = I_VALUE_MAX;
			}
		}
		// PID
		pid =((s_point / m_point) * pid_p ) + (calc_i * pid_i ) + ( err * pid_d );
	}
 }
 else
 {
	// Cool
	pid = 0;
	if( ct_i++ > ( I_COUNT_MAX * I_COUNT_DEC ) )	// Slower decrease of value
	{
	   ct_i = 0;									// Counter slows the response of I
	   if( calc_i != 0 )
			calc_i--;
	}
 }

 if( pid > 100) 
	pid = 100;
 if( m_point > ( s_point + HIGH_BAND ) )
	pid = 0;
}


//===================================================================
//	Saját PID controller
void pid_kit_calc( void )
{
	double err, pp, pi, pd;

	// Error
	err = s_point - m_point;

	// P
	pp = pid_p * err;
		
	// I
	pi = pid_i * ( err + err_1 + err_2 );
			
	// D
	pd = pid_d * ( m_point  - m_point_1 );
	
	// PID
	pid = pp + pi + pd;
	
	if( pid > PID_MAX )
	{
		pid = PID_MAX;
	}
	else if( pid < PID_MIN )
	{
		pid = PID_MIN;
	}
	
	// Save
	err_2 = err_1;
	err_1 = err;
	m_point_1 = m_point;
}



//******************************************************************************
//  MAIN
void main( void )
{

	pid_tak_init();


//	pid_tak_calc();			// -9 a PID változás
//	pid_book_calc();		// 414
//	pid_young_calc();		// 6042
//	pid_sim_calc(); 		// 70
//	pid_hc_calc();			// 104
//	pid_kit_calc();			// 116
		
}	// main()