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

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

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

#define PID_MAX			100		// pid max %
#define PID_MIN 		  0		// pid min %

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

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

// pid_kit
double m_point_1 = 64.4;	// PV[k-1]
double err_1 = 0.1;			// Error[-1]

//===================================================================
//	Saját PID controller
void pid_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 );
			
	// D
	pd = pid_d * ( m_point - m_point_1 );
	
	// PID
	pid = pp + pi + pd;
	
	// Save
	err_1 = err;
	m_point_1 = m_point;
}



void putch( unsigned char ch )
{
	while( ! TRMT1 )
		continue;
	TXREG1 = ch;
}

void USART_init( void )
{
	TRISC6 = 0;
	SPEN1 = 1;
	TXSTA1 = 0b10100010;
}

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

USART_init();

printf("%5.2f\n", pid );

m_point = 64.5;
pid_calc();
printf("%5.2f\n", pid );

m_point = 64.6;
printf("%5.2f\n", pid );

m_point = 64.7;
pid_calc();
printf("%5.2f\n", pid );

m_point = 64.8;
pid_calc();
printf("%5.2f\n", pid );

m_point = 64.9;
pid_calc();
printf("%5.2f\n", pid );

m_point = 65;
pid_calc();
printf("%5.2f\n", pid );

printf("End\n" );

}	// main()