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

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

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

// 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_watt
double m_point_1 = 64.4;	// PV[k-1]
double pi_1 = 230;

/*
P tag erősítés(arányos szabályzás):
Y_P=(SP-CV)*P
I tag korrekció:
Y_I=Y_I+(SP-CV)/I
D tag "ellenkormányzás": (Cve=előző hőfok)
Y_D=(Cve-CV)/D
Beavatkozó jel:
Y=Y_P+Y_I+Y_D
*/
//===================================================================
//	watt PID controller
void pid_calc( void )
{
	double err, pp, pi, pd;

	// Error
	err = s_point - m_point;

	// P
	pp = pid_p * err;
		
	// I
	pi = pi_1 + ( err ) / pid_i;
			
	// D
	pd = ( m_point_1 - m_point ) / pid_d;
	
	// PID
	pid = pp + pi + pd;
	
	// Save
	m_point_1 = m_point;
	pi_1 = pi;
}




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()