#include "OB6710.h"

extern void Test_PI_P(void);
extern void Test_PI_ERROR(void);
extern void Test_PI_N_1(void);

unsigned char gValue=0;

unsigned char KI, KP, Loop, OVF;
int IN, TAR, OUT, OUT_Chip;
long LMT, ERROR, I_N, I_N_1, REG;

void main(void)
{
	P0M0 = 0x00;
	P0M1 = 0x00;
	
	Test_PI_P();
	Test_PI_ERROR();
	Test_PI_N_1();
	while(1);
}

void Test_PI_P(void)
{
	//ERROR
	KI = 10;
	KP = 10;
	TAR = 0x160;
	I_N_1 = 0x00;
  IN = 0x00;
	PIPARLL = (unsigned char)(I_N_1 >> 0x00);
	PIPARLH = (unsigned char)(I_N_1 >> 0x08);
	PIPARHL = (unsigned char)(I_N_1 >> 0x10);

	PILMTLL = 0xFF;
	PILMTLH = 0xFF;
	PILMTHL = 0x5F;
	LMT = PILMTHL;
	LMT = (LMT << 0x08) | PILMTLH;
	LMT = (LMT << 0x08)| PILMTLL;
	
	for(Loop = 0x00;Loop<80;Loop++)
	{
  	OVF = 0x00;
		IN = IN + (Loop * 5);
		KI = KI + (Loop * 2);
    KP = KP + (Loop * 3);
    TAR = TAR + (Loop * 7);
		//-----------------------------------------
	  
		PIINL   = (unsigned char)IN;
  	PIINH   = (unsigned char)(IN >> 0x08);		
	  PIKI    = KI;
  	PIKP    = KP;
	  PITARL  = (unsigned char)TAR;
	  PITARH  = (unsigned char)(TAR >> 0x08);
		//----------------------------------------------
		ERROR = TAR - IN;
		I_N = (KI* ERROR) + I_N_1;
		if(I_N>=LMT)
		{
			OVF = OVF | 0xA0;
			I_N = LMT;		
		}
		I_N_1 = I_N;
		REG =  (KP * ERROR) + I_N ;
		if(REG >= LMT)
		{
			OVF = OVF | 0xC0;
			REG = LMT;
		}
		OUT = REG>>0x08;
		//---------------------------------
  	PICON = PICON | 0x01;
	
		while(1)
		{
			if( (PICON & 0x01) == 0x00)
				break;
		}
		OUT_Chip = PIOUTH;
		OUT_Chip = (OUT_Chip<< 0x08) |  PIOUTL;
		//---------------------------------------------
		if(OUT_Chip != OUT)
		{
			while(1)
			{
				P0_0 = ~P0_0;
			}
		}
		//---------------------------------------------
		if(PICON != OVF)
		{
			while(1)
			{
				P0_0 = ~P0_0;
			}		
		}
		
		if(I_N>=LMT)
		{
		  I_N_1 = 0x00;
			PIPARLL = 0x00;
			PIPARLH = 0x00;
			PIPARHL = 0x00;		
		}
		
		PICON = PICON ; //Clear OVF
	}
}

void Test_PI_ERROR(void)
{
	int Tar_Buff, In_Buff;

	//ERROR
	KI = 10;
	KP = 10;
  PIKI    = KI;
 	PIKP    = KP;
	I_N_1 = 0x00;
	PIPARLL = (unsigned char)(I_N_1 >> 0x00);
	PIPARLH = (unsigned char)(I_N_1 >> 0x08);
	PIPARHL = (unsigned char)(I_N_1 >> 0x10);

	PILMTLL = 0xFF;
	PILMTLH = 0xFF;
	PILMTHL = 0x5F;
	LMT = PILMTHL;
	LMT = (LMT << 0x08) | PILMTLH;
	LMT = (LMT << 0x08)| PILMTLL;
	In_Buff = 0x00;
	while(1)
	{
		Tar_Buff = 00;
		while(1)
		{
			P0_7 = ~P0_7;
			OVF = 0x00;
			IN = In_Buff;
			TAR = Tar_Buff;
		//-----------------------------------------
		PIINL   = (unsigned char)IN;
  	PIINH   = (unsigned char)(IN >> 0x08);
			
	  PITARL  = (unsigned char)TAR;
	  PITARH  = (unsigned char)(TAR >> 0x08);

		I_N_1 = 0x00;	
		PIPARLL = (unsigned char)(I_N_1 >> 0x00);
		PIPARLH = (unsigned char)(I_N_1 >> 0x08);
		PIPARHL = (unsigned char)(I_N_1 >> 0x10);

			//----------------------------------------------
		if(TAR>=0x8000)
		{
			ERROR = TAR | 0xFFFF0000;
			ERROR = ERROR - IN;
			if(ERROR< -32768)
			{
				ERROR =-32768; 	
				OVF = OVF | 0x90;
			}
		}
		else
		{
			ERROR = TAR;
			ERROR = ERROR - IN;
			if(ERROR>=0x8000)
			{
				ERROR = 0x7FFF;
				OVF = OVF | 0x90;
			}
		}
		I_N = (KI* ERROR) + I_N_1;
		if(I_N>=LMT)
		{
			OVF = OVF | 0xA0;
			I_N = LMT;		
		}
		I_N_1 = I_N;
		REG =  (KP * ERROR) + I_N ;
		if(REG >= LMT)
		{
			OVF = OVF | 0xC0;
			REG = LMT;
		}
		OUT = REG>>0x08;
		//---------------------------------
  	PICON = PICON | 0x01;
	
		while(1)
		{
			if( (PICON & 0x01) == 0x00)
				break;
		}
		OUT_Chip = PIOUTH;
		OUT_Chip = (OUT_Chip<< 0x08) |  PIOUTL;
		//---------------------------------------------
		if(OUT_Chip != OUT)
		{
			while(1)
			{
				P0_0 = ~P0_0;
			}
		}
		//---------------------------------------------
		if(PICON != OVF)
		{
			while(1)
			{
				P0_0 = ~P0_0;
			}		
		}
		
		if(I_N>=LMT)
		{
		  I_N_1 = 0x00;
			PIPARLL = 0x00;
			PIPARLH = 0x00;
			PIPARHL = 0x00;		
		}
		
		PICON = PICON ; //Clear OVF
		if(Tar_Buff >= (0xFFFE - 345))
			break;
		Tar_Buff = Tar_Buff+345;
		}
		if(In_Buff >= (0xFFFE - 123))
			break;

	  In_Buff = In_Buff + 123;
	}
}

void Test_PI_N_1(void)
{
	long Loop;

	//ERROR
	TAR = 0x160;
	IN= 5;
	KI = 10;
	KP = 10;
  PIKI    = KI;
 	PIKP    = KP;
	I_N_1 = 0x00;

	PILMTLL = 0xFF;
	PILMTLH = 0xFF;
	PILMTHL = 0x5F;
	LMT = PILMTHL;
	LMT = (LMT << 0x08) | PILMTLH;
	LMT = (LMT << 0x08)| PILMTLL;

	PIINL   = (unsigned char)IN;
 	PIINH   = (unsigned char)(IN >> 0x08);
			
  PITARL  = (unsigned char)TAR;
  PITARH  = (unsigned char)(TAR >> 0x08);	
	
  Loop = 0x00;

	I_N_1 = 0x00;
		while(1)
		{
			P0_7 = ~P0_7;
			OVF = 0x00;
      I_N_1 = Loop;
		//-----------------------------------------

		 PIPARLL = (unsigned char)(I_N_1 >> 0x00);
		 PIPARLH = (unsigned char)(I_N_1 >> 0x08);
		 PIPARHL = (unsigned char)(I_N_1 >> 0x10);

			//----------------------------------------------

		ERROR = TAR- IN;
    if(I_N_1>=0x00800000)
				I_N_1 = I_N_1 |0xFF000000;	
		
		I_N = (KI* ERROR) + I_N_1;
		
		if( (I_N>=LMT) || (I_N<= -LMT))
		{
			OVF = OVF | 0xA0;
			if((I_N>=LMT))
				I_N = LMT;		
			if(I_N<= -LMT)
				I_N = -LMT;	
		}
		I_N_1 = I_N;
		REG =  (KP * ERROR) + I_N ;
		if(REG >= LMT)
		{
			OVF = OVF | 0xC0;
			REG = LMT;
		}
		OUT = REG>>0x08;
		//---------------------------------
  	PICON = PICON | 0x01;
	
		while(1)
		{
			if( (PICON & 0x01) == 0x00)
				break;
		}
		OUT_Chip = PIOUTH;
		OUT_Chip = (OUT_Chip<< 0x08) |  PIOUTL;
		//---------------------------------------------
		if(OUT_Chip != OUT)
		{
			while(1)
			{
				P0_0 = ~P0_0;
			}
		}
		//---------------------------------------------
		if(PICON != OVF)
		{
			while(1)
			{
				P0_0 = ~P0_0;
			}		
		}
		
		if(I_N>=LMT)
		{
		  I_N_1 = 0x00;
			PIPARLL = 0x00;
			PIPARLH = 0x00;
			PIPARHL = 0x00;		
		}
		
		PICON = PICON ; //Clear OVF

			Loop = Loop+123;
		if(Loop >= 0x00FFFFFF)
			break;		
	}
}