//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度


//方程的系數

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


//輸出控制頻率
//輸入參數:	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;
}




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;

	u32 ulFX,ulFY;

	SysInit();

	
	ulFX=45000;
	ulFY=Fy0;
	OutputFrequency(ulFY,ulFX);
	//OutputFrequency(40000,ulFX);
	//OutputFrequency(70000,ulFX);
	//OutputFrequency(40000,ulFX);
	//OutputFrequency(70000,ulFX);
	//OutputFrequency(40000,ulFX);
	//OutputFrequency(70000,ulFX);

	while(1)
	{
		OutputFrequency(40000,ulFX);
		NOP();
		OutputFrequency(70000,ulFX);
		NOP();
	}






	
	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;
	//dFy=0.061242;
	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();
}