#include "2407bit.h"
#include "2407c.h"
#include "2407e.h"
#include "Pid_reg1.h"
#include "display.h"
#include "Speed_Reg.h"
#include "Postion_reg.h"
void IoInit(void);
void KeyScan(void);
unsigned int TimeFlag;
unsigned int dispclk=0;
unsigned int StopKeyCnt=0;
unsigned int KeyFlag[5]={0,0,0,0,0};
unsigned int KeyCnt[5]={0,0,0,0,0};
enum KeyTable {SW,ADD,SUB,XIN1,XIN2};
PIDREG1 pid0 = PIDREG1_DEFAULTS;
SPEEDPID speed0={
			&pid0,		//Point to PID calc
				0,		//Speed Freeback
				0x01ff,	//Init speed
				0x100,	//Speed
				(void (*)(int))EVA_PWM1_Init,	//Function for init
				(void (*)(int))EVA_PWM1_Updata	//Function for updata
			};

POSTIONPID	postion0={
					 &speed0,			//Speed Reg
					 0,					//Absolute postion
					 0xfffff,			//Reference input
					 0,					//Free back	input
					 0,					//Postion Reg Output
					 0x10,				//Speed
					 1023,				//Max Speed
					 -1023,				//Min Speed
					 0,					//E1
					 0,					//E2
					 130,				//Kp
					 250,				//Ki
					 0,					//Kd
					 (int (*)(int))PostionPid	//Updata Function
			};
main()
{
	unsigned int Led;
	asm(" SETC INTM");		//Disable all the interrupt
	asm(" SETC SXM");		//Disable sign extend
	asm(" CLRC OVM");		//Overflow normally
	asm(" CLRC CNF");		//B0 served as data ram
	*SCSR1=0x81FE;			//CLKOUT is Dsp's Clk Clk=OscX4		 			
	*WDCR=0x00E8;			//Wacth dog disable
	*IMR=0x0032;			//Enable INT2 INT6 INT5
	*IFR=0xFFFF;
	IoInit();
	DispInit();
	speed0.init();
	T3Init();
	

	asm(" CLRC SXM");
	asm(" CLRC INTM");
	while(KeyCnt[ADD]<5)
 	{
 	  TimeFlag=0;
 	  StopKeyCnt=0;
 	  if(dispclk==25)
 	  {
 	  Led=(*PCDATDIR)&0x0001;	//Change Led 
	  Led=Led^0x0001;
	  *PCDATDIR=(*PCDATDIR&0xfffe)|Led;	
 	  //Display(postion0.PostionF);
 	  Display(speed0.Qep_F);
 	  dispclk=0;
 	  }                      
 	  //postion0.Updata(&postion0);
 	  speed0.updata(&speed0);
 	  KeyScan();
 	  while(!TimeFlag);
 	}	
	
	
	while(1)
 	{
 	  StopKeyCnt=0;
 	  TimeFlag=0;
 	  if(dispclk==25)
 	  {
 	  Led=(*PCDATDIR)&0x0001;	//Change Led 
	  Led=Led^0x0001;
	  *PCDATDIR=(*PCDATDIR&0xfffe)|Led;	
 	  Display(postion0.PostionF);
 	  //Display(speed0.Qep_F);
 	  dispclk=0;
 	  }                      
 	  postion0.Updata(&postion0);
 	  //speed0.updata(&speed0);
 	  
 	  KeyScan();
 	  if(KeyCnt[XIN1]>4)
 	  	{
 	  	 postion0.Speed=0;
 	  	 postion0.PostionR=postion0.PostionF;}
 	  while(!TimeFlag);
 	}	
	





}
void interrupt gisr2 (void)
{
	switch ((*PIVR))
	{
	 case 0x002F  :
	 	{
	 		(*EVBIFRA)|=1<<T1PINT;		//Clear interrupt flag
	 		(*T3CNT)=0x0000;
			
			 speed0.Qep_F=*T4CNT;
			 postion0.PostionF+=(int)*T4CNT;
			 //*T2CNT=0;
			 *T4CNT=0;
			 dispclk++;
			 TimeFlag=1;
			 				
	 	}
	 	break;
	 }
	
}



void interrupt gisr5(void)
{
	/*int temp;
	temp=0;
	switch(*PIVR)
	{
		case 0x0006:		//RX中斷
			{
				temp=*SCIRXBUF;
				if(!((*SCICTL1)&0x04))
				{
					//rxd data
					*(RxdBuf+RxdIndex)=temp;
					  RxdIndex++;
					if(RxdIndex==RxdNum)
					 {
					 	RxdIndex=0;
					 	*SCICTL1=0x0037;	
					 }
					 	
				}
				
				else if(temp==TxdAdd)
				{
					//txd data
					//txd enable
					*SCICTL2=((*SCICTL2)|0x0001)&0xfffd;		//發送中斷使能
					(*PFDATDIR)|=(1<<EN485);					//485 發送使能
					*SCITXBUF=*(TxdBuf+TxdIndex);
					TxdIndex++;
				}
				else if(temp==RxdAdd)
			 		*SCICTL1=0x0033;
			 }
			 break;
		 case 0x0007:		//TX中斷		
			{
			//txd data
				if(TxdIndex<(TxdNum+1))
				{
					*SCITXBUF=*(TxdBuf+TxdIndex);
					TxdIndex++;
				}
				else
				{
					TxdIndex=0;
					*SCICTL2=(*SCICTL2&0x0fffe)|0x0002;	//接收中斷使能
									
					(*PFDATDIR)&=~(1<<EN485);			//485 接收使能
					
				}	
			}
			break;
		default:break;
		
	}*/
}

void interrupt gisr6(void)
{
/*	int	i;
	switch(*PIVR)
	{
		case 0x0004:				//AD中斷
			{
			
				*EVBIFRA=1<<T1UFINT;		//Clear T1UFINT interrupt	
				(*ADCTRL2)|=0x0002;			//Clear AD interrupt
				for(i=0;i<8;i++)
				 AdValue[i]=*(RESULT8+i)>>6;
			 }
			 break;
	}*/			 
}
void IoInit(void)
{
	(*MCRA)&=~((1<<IN_DSP0)|(1<<IN_DSP1)|(1<<IN_DSP2)|(1<<IN_DSP3)|(1<<IN_DSP4)|(1<<IN_DSP5)|(1<<IN_DSP6)|(1<<IN_DSP7));
	(*MCRB)&=~(1<<IOPC0);
	(*MCRC)&=~((1<<OUT_DSP0)|(1<<OUT_DSP1)|(1<<OUT_DSP2)|(1<<OUT_DSP3)|(1<<OUT_DSP4)|(1<<OUT_DSP5)|(1<<OUT_DSP6)|(1<<OUT_DSP7));
	(*PADATDIR)&=~(IN_DSP0+8);
	(*PBDATDIR)=0x0000;
	(*PCDATDIR)|=(1<<(IOPC0+8))|(1<<IOPC0);
	(*PEDATDIR)|=((1<<(OUT_DSP3+8))|(1<<(OUT_DSP4+8))|(1<<(OUT_DSP5+8)));
	(*PFDATDIR)|=((1<<OUT_DSP0)|(1<<OUT_DSP1)|(1<<OUT_DSP2)|(1<<OUT_DSP6)|(1<<OUT_DSP7));
	(*PFDATDIR)&=0xff00;
}
void KeyScan()
{
	//Key in IOPA5
	unsigned int temp,i,j;
	
  for(j=1;j<5;j++)
	{	
		KeyCnt[j]=0;
		for(i=0;i<5;i++)
		{	
	 		temp=*PBDATDIR;
	  		if((~temp)&(1<<(IN_DSP1-8+j-1)))
			KeyCnt[j]++;
		}
		if(KeyCnt[j]>4)
		 KeyFlag[j]=1;
	}	
		
}