//ICC-AVR application builder : 2007-10-30 15:43:17
#include "public.h"


#define Fy0 	61242
#define FyL 	60742
#define FyH 	61742

#define RadPer1MHz 	0.001

#define Rad0 	0.087266	//5度


//方程的系數(shù)

#define xishuA	4.9201506335
#define xishuB	-11.4057818550
#define xishuC	-4.9964661567

//const u8 ucHEX2BCD[3000][3];


u8 ucHex2BCD(u8 ucIn)//  十六進制轉(zhuǎn)BCD
{
	u8 uctemp;

	uctemp=ucIn%10;
	uctemp=uctemp|(((ucIn%100)/10)<<4);

	return uctemp;

}
void HEX2BCD(u32 ulHex,u8* pucOutBuff)
{
	u8 uctemp;

	pucOutBuff[2]=ulHex%10;
	ulHex=ulHex/10;
	pucOutBuff[2]=pucOutBuff[2]|((ulHex%10)<<4);
	ulHex=ulHex/10;
	pucOutBuff[1]=ulHex%10;
	ulHex=ulHex/10;
	pucOutBuff[1]=pucOutBuff[1]|((ulHex%10)<<4);
	ulHex=ulHex/10;
	pucOutBuff[0]=ulHex%10;
	ulHex=ulHex/10;
	pucOutBuff[0]=pucOutBuff[0]|((ulHex%10)<<4);

}

//輸出控制頻率
//輸入?yún)?shù):	ulFX X軸頻率,單位kHz
//		ulFY Y軸頻率,單位kHz
void OutputFrequency(u32 ulFX,u32 ulFY)
{
	u8 ucFX[3],ucFY[3];
	if((ulFX<40000)||(ulFX>70000))	return;
	if((ulFY<40000)||(ulFY>70000))	return;
	
	HEX2BCD(ulFX,ucFX);
	HEX2BCD(ulFY,ucFY);


	


	DISABLEXOUT;
	DISABLEYOUT;

	SPISend(ucFX[0]);
	SPISend(ucFX[1]);
	SPISend(ucFX[2]);
	SPISend(ucFY[0]);
	SPISend(ucFY[1]);
	SPISend(ucFY[2]);

	ENABLEXOUT;
	ENABLEYOUT;
}


void OutputFrequency1(void)
{
	
	DISABLEXOUT;
	DISABLEYOUT;

	SPISend(0x06);
	SPISend(0x10);
	SPISend(0);
	SPISend(0);
	SPISend(0);
	SPISend(0);

	ENABLEXOUT;
	ENABLEYOUT;
}

void OutputFrequency2(void)
{

	DISABLEXOUT;
	DISABLEYOUT;

	SPISend(0x07);
	SPISend(0x00);
	SPISend(0x00);
	SPISend(0);
	SPISend(0);
	SPISend(0);

	ENABLEXOUT;
	ENABLEYOUT;
}





void OutputFrequency3(u8 ucFX)
{
	DISABLEXOUT;
	DISABLEYOUT;

	SPISend(0x06);
	SPISend(ucFX);
	SPISend(0);
	SPISend(0);
	SPISend(0);
	SPISend(0);

	ENABLEXOUT;
	ENABLEYOUT;
}


u32 ulGetFrequency(u32 ulNowF,double dNowRad2)
{
	double dNowF,dNextF;
	u32 ulNextF;
	
	dNowF=((double)ulNowF)/(double)1000000;

	dNextF=-1-xishuA*dNowF-xishuB*dNowRad2;
	dNextF=dNextF/xishuC;
	
	ulNextF=dNextF*1000000;
	return ulNextF;
}



void main(void)
{
	u32	i=0xFFFFFF;
	u16 j,k;
	u16 usADDATA[4];
	double dADDATA[4];
	double dEx,dEy;
	double dTotalData;

	double dEy1T,dEy2T,dEyTotal;

	double dDeltaRad,dRad2;

	double dFy;

	s32 slEy;
	
	u32 ulFX,ulFY;

	u8 ucFlag;


	u8 ucFX=99;


	SysInit();

	
	ulFX=45000;
	ulFY=Fy0;


	ulFY=61000;
	OutputFrequency(ulFY,ulFX);

	
	//ulFY=70000;
	//OutputFrequency3(ucFX);

/*
	ucFlag=0;
	while(1)
	{
		if(ucFlag)	
		{
			OutputFrequency2();
			ucFlag=0;
		}
		else						
		{
			OutputFrequency1();
			ucFlag=1;
		}
		delayus(200);
		delayus(100);
		//delayus(200);
		//delayus(200);
		//delayus(200);
		//delayus(200);
		//delayms(10);
	}
	*/
	delayms(10);

	
	while(1)
	{
		NOP();
		delayms(3);
		usAD_GetData(usADDATA);
		//dADDATA[0]=(double)usADDATA[0];
		//dADDATA[1]=(double)usADDATA[1];
		//dADDATA[2]=(double)usADDATA[2];
		//dADDATA[3]=(double)usADDATA[3];
		//dTotalData=dADDATA[QDA]+dADDATA[QDB]+dADDATA[QDC]+dADDATA[QDD];
		

		//dEx=(dADDATA[QDA]+dADDATA[QDD]-dADDATA[QDB]-dADDATA[QDC])/dTotalData;
		//dEy=(dADDATA[QDA]+dADDATA[QDB]-dADDATA[QDC]-dADDATA[QDD])/dTotalData;

		slEy=usADDATA[QDA];
		slEy=slEy+usADDATA[QDB];
		slEy=slEy-usADDATA[QDC];
		slEy=slEy-usADDATA[QDD];
		
		if(slEy!=0)
		{
			if(slEy>0) 	
			{	
				ulFY=ulFY+100;
				if(ulFY>70000)	ulFY=70000;
			}
			else 		
			{	
				ulFY=ulFY-100;
				if(ulFY<40000)	ulFY=40000;
			}
			OutputFrequency(ulFY,ulFX);
		}
		
		NOP();
		NOP();
	}


	while(1)
	{
		usAD_GetData(usADDATA);
		if(usADDATA[QDD]>0x6FFF)	
			ucFX=ucFX+1;
		else		//if(usADDATA[QDD]<10)					
			ucFX=ucFX-1;
		delayms(3);
		OutputFrequency3(ucHex2BCD(ucFX));

	}


	
	while(1)
	{
		usAD_GetData(usADDATA);
		if(usADDATA[QDD]>100)	
			OutputFrequency2();
		else		if(usADDATA[QDD]<10)					
			OutputFrequency1();
		//delayms(3);

	}




	while(1)
	{
		OutputFrequency1();
		delay(0xFF);
		delay(0xFF);
		OutputFrequency2();
		delay(0xFF);
		delay(0xFF);
	}






	
	dEy1T=0;
	dEy2T=0;
	for(i=0;i<1000;i++)
	{
		ulFY=60500;
		OutputFrequency(ulFY,ulFX);
		delayms(10);
		usAD_GetData(usADDATA);
		dADDATA[0]=(double)usADDATA[0];
		dADDATA[1]=(double)usADDATA[1];
		dADDATA[2]=(double)usADDATA[2];
		dADDATA[3]=(double)usADDATA[3];
		dTotalData=dADDATA[QDA]+dADDATA[QDB]+dADDATA[QDC]+dADDATA[QDD];
		

		//dEx=(dADDATA[QDA]+dADDATA[QDD]-dADDATA[QDB]-dADDATA[QDC])/dTotalData;
		dEy=(dADDATA[QDA]+dADDATA[QDB]-dADDATA[QDC]-dADDATA[QDD])/dTotalData;
		dEy1T=dEy1T+dEy;

		ulFY=61500;
		OutputFrequency(ulFY,ulFX);
		delayms(10);
		usAD_GetData(usADDATA);
		dADDATA[0]=(double)usADDATA[0];
		dADDATA[1]=(double)usADDATA[1];
		dADDATA[2]=(double)usADDATA[2];
		dADDATA[3]=(double)usADDATA[3];
		dTotalData=dADDATA[QDA]+dADDATA[QDB]+dADDATA[QDC]+dADDATA[QDD];
		

		//dEx=(dADDATA[QDA]+dADDATA[QDD]-dADDATA[QDB]-dADDATA[QDC])/dTotalData;
		dEy=(dADDATA[QDA]+dADDATA[QDB]-dADDATA[QDC]-dADDATA[QDD])/dTotalData;
		dEy2T=dEy2T+dEy;
	}

	NOP();
	dEy1T=dEy1T/1000;
	dEy2T=dEy2T/1000;
	NOP();
	NOP();
	dEx=(dEy1T+dEy2T);
	dEyTotal=(dEy1T-dEy2T);
	NOP();
	NOP();


	ulFY=Fy0;
	OutputFrequency(ulFY,ulFX);
	while(1)
	for(i=0;i<1000;i++)
	{
		delayms(10);
		usAD_GetData(usADDATA);
		dADDATA[0]=(double)usADDATA[0];
		dADDATA[1]=(double)usADDATA[1];
		dADDATA[2]=(double)usADDATA[2];
		dADDATA[3]=(double)usADDATA[3];
		dTotalData=dADDATA[QDA]+dADDATA[QDB]+dADDATA[QDC]+dADDATA[QDD];
		
		dEy=(dADDATA[QDA]+dADDATA[QDB]-dADDATA[QDC]-dADDATA[QDD])/dTotalData;

		dDeltaRad=(dEy*RadPer1MHz)/dEyTotal;//偏移的弧度

		dRad2=Rad0-dDeltaRad;
		ulFY=ulGetFrequency(ulFY,dRad2);
		OutputFrequency(ulFY,ulFX);
	}

	while(1)
		NOP();
	


	

	while(1)
	{
		NOP();
		//ulAD_GetData(ulADDATA);
		usAD_GetData(usADDATA);
		dADDATA[0]=(double)usADDATA[0];
		dADDATA[1]=(double)usADDATA[1];
		dADDATA[2]=(double)usADDATA[2];
		dADDATA[3]=(double)usADDATA[3];
		dTotalData=dADDATA[QDA]+dADDATA[QDB]+dADDATA[QDC]+dADDATA[QDD];
		

		dEx=(dADDATA[QDA]+dADDATA[QDD]-dADDATA[QDB]-dADDATA[QDC])/dTotalData;
		dEy=(dADDATA[QDA]+dADDATA[QDB]-dADDATA[QDC]-dADDATA[QDD])/dTotalData;

		
		if(dEy!=0)
		{
			if(dEy>0) 	
			{	
				if(ulFY<70000)	ulFY=ulFY+100;
			}
			else 		
			{	
				if(ulFY>40000)	ulFY=ulFY-100;
			}
		//	OutputFrequency(ulFY,ulFX);
		}
		
		NOP();
		NOP();
	}


	while(1)
		NOP();
}