#include "2407c.h"                    
#include "PID.h"
#include "scancode.h"
#include "ctr.h"
ioport unsigned int port8000;	
ioport unsigned int port8001;	
ioport unsigned int port8002;	
ioport unsigned int port8007;
ioport unsigned int port8003;	

void Delay(unsigned int nTime);
void interrupt time(void);
char ConvertScanToChar(unsigned char cScanCode);
void interrupt gptime1(void);     
void gp_init(void);      
void PIDControl(int rk,int yk);
void ShowParameters();
//void key(void);
ioport unsigned int port8001;	
unsigned int uWork,nCount,uN,uN1,nCount1=0,nDir;
int i=0,nnn;
unsigned int nCount,nCount1,nCount2,nCount3,nCount_1=1,nJSSpeed,nJSSpeed1=1,uWork,uN;
unsigned int uLBD;
float a=0.6f,b=0.2f,c=0.1f,duk;
int ek,ek1,ek2,tz;
int nInput;
int nSSS,pwm;
int md,wc,speed[100];
unsigned int nScreenBuffer[1024];
unsigned int _nClearKey,_ledx[8],_ledbuf[8],_ledkey[11][8];
char cKey,cOldKey;
unsigned int nScanCode;
unsigned int uWork1,uWork_1=0,nSpeed;
unsigned int uPort8000;
int sp,sp1=1,lj;
float ljh;
char strInput[4];
int i,w1,w2,w3;
main()
{   
    
	
	asm(" setc INTM");
	uLBD=nnn=0;
	nCount=nCount1=nCount2=nCount3=nJSSpeed=0;
	for ( sp=0;sp<100;sp++ )	speed[sp]=0;
	sp=nSSS=nCount=nCount1=nCount2=nJSSpeed=0; cKey=0; nInput=tz=wc=0;
	ek=ek1=ek2=0;
	for ( uWork=0;uWork<4;uWork++ )	strInput[uWork]=0;
	uN=40; md=30; pwm=60;
	
	port8000=0;
	port8000=0x80;
	port8000=0;
	port8007=0;			// 關閉東西方向的交通燈
	port8007=0x40;	// 關閉南北方向的交通燈
	uWork1=port8002;	// 清除鍵盤緩沖區

    CTRInit();
	
    *WSGR=0x09;
	*WDCR=0x6f;
	*WDKEY=0x5555;
	*WDKEY=0xaaaa;    
	*SCSR1=0x81fe;    //40m      
	uWork=(*MCRC);
	uWork&=0x0ffc0;	/* PWM11/IOPE5,PWM12/IOPE6 for normal I/O ports */
	(*MCRC)=uWork;
	gp_init();   
	*IMR=0x3;
	*IFR=0xffff;    
	nCount=0;
	uWork=(*WSGR);
	uWork&=0x0fe39;
	(*WSGR)=uWork;
	
	// 設置顯示參數和內容
	LCDSetDelay(1);		// 設置延時等待參數
	LCDSetScreenBuffer(nScreenBuffer);	// 顯示緩沖區
	LCDTurnOn();			// 打開顯示
	LCDCLS();				// 清除顯示內存   
	LCDPutCString(str1,0,63,8,0);
	LCDPutCString(str2,0,47,2,1);
	LCDPutCString(str3,68,47,2,1);
	LCDPutCString(str6,0,31,2,1);
	LCDPutCString(str5,68,31,2,1);
	LCDPutCString(str7,0,15,3,1);
	LCDPutCString(str4,68,15,2,1);
	ShowParameters();		// 參數顯示
	asm(" clrc INTM");         
	Delay(128);
	port8000=0x81;
	//port8007=0x0c4;//使能直流電機
	for(;;)
	{

		/*if ( nCount_1==0 )	// 讀取鍵盤標志
		{ 
		  nCount_1=1;
		  key();
		}*/
		if ( nJSSpeed1==0 )	// 讀取速度標志
		{  asm(" setc INTM");
			nJSSpeed1=1;
			nSSS=port8003;	// 從端口讀取速度計數
			nSSS&=0x0ff;
			if ( nSSS>=0 && nSSS<100 )	// 合法性檢測
			{
				speed[sp]=nSSS; 		// 讀取66個計數值
				sp++; sp%=16;
				if(sp==0) sp1=0;
			}
			if ( sp1==0 )	// 是否已經讀了66個速度？
			{				// 以下求速度平均值
			    sp1=1;
				lj=0; ljh=0;
				for ( i=0;i<16;i++ )
				{
					if ( speed[i]>=0 && speed[i]<150 )
					{
						ljh+=speed[i];
						lj++;
					}
				}
				wc=( lj==0 )?(0):(ljh/lj);
				if ( wc>150 )
				{	
					wc=0;
				}
				nCount3++; nCount3%=3;
				if ( nCount3==2 )		
				{
					PIDControl(md,wc);	// 調用PID算法控制程序進行控制
					uN=(100-pwm)/2;			// 利用占空比調整控制
					ShowParameters();	// 顯示各參數值到LCD
				}
			}
		}
			asm(" clrc INTM");  
	}
	port8000=0;
	port8000=0x80;
	port8000=0;
	exit(0);
}               
#define Hz200 50
#define Hz1 10000
void interrupt gptime1(void)
{  int fuzhi=0;
   uWork=(*PIVR);
   switch(uWork)
   {  
   		case 0x27:
   		{
			(*EVAIFRA)=0x80;
			//key();
			uWork=(*PEDATDIR);
			uWork|=0x6000;//設置pwm11和pwm12為io輸出模式
	        if(nCount1<=uN) 
	        {uWork=(*PEDATDIR);
	        uWork&=0x60DF;
	        (*PEDATDIR)=uWork;}
	        if(nCount1>uN) 
	        {uWork=(*PEDATDIR);
	        uWork|=0x6020;
	        (*PEDATDIR)=uWork;}
	         nCount1++; nCount1%=Hz200;
            if ( nCount2==0 )
            {
	        	
	        	if(nnn==0) {uWork=(*PEDATDIR);
			uWork|=0x6000;uWork&=0x60BF;(*PEDATDIR)=uWork;}
	        	if(nnn==1) {uWork=(*PEDATDIR);
			uWork|=0x6000;uWork|=0x6040;(*PEDATDIR)=uWork;}
	        	nnn=1-nnn;
            }
  			nCount2++; nCount2%=Hz1;
			nJSSpeed++; nJSSpeed%=12000/8;	// 讀取速度標志
			if(nJSSpeed==0) nJSSpeed1=0;
			//nCount++; nCount%=51200/10;	
			//if(nCount==0) nCount_1=0;	
   		   	break;
   		}
   }
}                       

void gp_init(void)
{
   *EVAIMRA = 0x80;
   *EVAIFRA = 0xffff;
   *GPTCONA = 0x0100;
   //*T1PR    = 0xe80;//燒到flash中運行用此參數
   *T1PR    = 0x200; //ram中運行用此參數
   *T1CNT   = 0;
   *T1CON   = 0x1040;
}



void Delay(unsigned int nDelay)
{
	int i,j,k;
	
	for ( i=0;i<nDelay;i++ )
		for ( j=0;j<16;j++ )
			k++;
}

// PID算法控制子程序-------------------------------------------------------------------------
void PIDControl(int rk,int yk)
{
	ek=rk-yk;
	duk=a*ek+b*ek1+c*ek2;		// 計算控制輸出
	ek2=ek1; ek1=ek;
	if ( duk>10 )	duk=3;		// 幅度限制
	tz=(int)duk;
	pwm+=tz;					// 計算當前占空比
	if ( pwm<0 )	pwm=0;
	else if ( pwm>99 )	pwm=99;
}
// 顯示參數到LCD
void ShowParameters()
{
	int w1,w2,w3;
	
	w1=md%1000/100; w2=md%100/10; w3=md%10;
	LCDPutString(numbers+w1*8,36,47,1,1);
	LCDPutString(numbers+w2*8,44,47,1,1);
	LCDPutString(numbers+w3*8,52,47,1,1);
	w1=wc%1000/100; w2=wc%100/10; w3=wc%10;
	LCDPutString(numbers+w1*8,104,47,1,1);
	LCDPutString(numbers+w2*8,112,47,1,1);
	LCDPutString(numbers+w3*8,120,47,1,1);
	if ( ek>=0 )
	{
		LCDPutString(numbers+88,36,31,1,1);
		w3=((int)ek)%100;
	}
	else
	{
		LCDPutString(numbers+96,36,31,1,1);
		w3=((int)(-ek))%100;
	}
	w1=w3%100/10; w2=w3%10;
	LCDPutString(numbers+w1*8,44,31,1,1);
	LCDPutString(numbers+w2*8,52,31,1,1);
	if ( tz>=0 )
	{
		LCDPutString(numbers+88,104,31,1,1);
		w3=tz;
	}
	else
	{
		LCDPutString(numbers+96,104,31,1,1);
		w3=(-tz);
	}
	w1=w3%100/10; w2=w3%10;
	LCDPutString(numbers+w1*8,112,31,1,1);
	LCDPutString(numbers+w2*8,120,31,1,1);
	w1=pwm%100/10; w2=pwm%10;
	LCDPutString(numbers+w1*8,44,15,1,1);
	LCDPutString(numbers+w2*8,52,15,1,1);
	LCDPutString(numbers+80,60,15,1,1);
	w1=uWork_1%1000/100; w2=uWork_1%100/10; w3=uWork_1%10;
	LCDPutString(numbers+w1*8,104,15,1,1);
	LCDPutString(numbers+w2*8,112,15,1,1);
    LCDPutString(numbers+w3*8,120,15,1,1);
	LCDRefreshScreen();
}