/*          */
/* ut1d.c */
/*          */
/* sumo robot control program 	*/
/* (c)1999 Takao Fujiwara 	*/


#include <3048f.h>

#define	IMFA0		ITU0.TSR.BIT.IMFA

#define	SW_A		P5.DR.BIT.B1
#define	SW_B		P5.DR.BIT.B2
#define	SW_C		P5.DR.BIT.B3

/* P4  LCD			*/
/* B5  B4  B3  B2  B1  B0  	*/
/* E   RS  D7  D6  D5  D4 	*/


#define	LCD_D4		P4.DR.BIT.B0
#define	LCD_D5		P4.DR.BIT.B1
#define	LCD_D6		P4.DR.BIT.B2
#define	LCD_D7		P4.DR.BIT.B3
#define	LCD_RS		P4.DR.BIT.B4
#define	LCD_E		P4.DR.BIT.B5


/* MOTOR CONTROL	*/
/* 			*/
/* PWM			*/
/* 0		ON	*/
/* 1		OFF	*/
/* 			*/
/* B	A		*/
/* 0	0	OFF	*/
/* 0	1	CW	*/
/* 1	0	CCW	*/
/* 1	1	BRAKE	*/

#define L_PWM		PA.DR.BIT.B6
#define R_PWM		PA.DR.BIT.B4
#define L_B		PA.DR.BIT.B3
#define L_A		PA.DR.BIT.B2
#define R_B		PA.DR.BIT.B1
#define R_A		PA.DR.BIT.B0

void init_io( void );
void wait_1sec( void );		/* wait 1 sec	*/
void wait_5sec( void );		/* wait 5 sec	*/

void init_LCD( void );
void LCD_H( unsigned char );
void LCD_L( unsigned char );
void LCD_set( unsigned char );
void LCD_disp( char * );
void LCD_cursor( int, int );
void clr_1( void );			/* LCD LINE 1 CLEAR */
void clr_2( void );			/* LCD LINE 2 CLEAR */
void delay450n( void );
void delay( unsigned int );

int mode(void);

void F(void);		/* 前前　*/
void B(void);		/* 後後　*/
void R(void);		/* 前後　*/
void L(void);		/* 後前　*/
void S(void);		/* 止止　*/
void N(void);		/* 自自　*/
void SB(void);		/* 止後　*/
void BS(void);		/* 後止　*/
void FS(void);		/* 前止　*/
void SF(void);		/* 止前　*/

void mode_1(void);
void mode_2(void);
void mode_3(void);
void mode_4(void);
void mode_5(void);
void mode_6(void);
void mode_7(void);
void mode_8(void);
void mode_9(void);

main()
{
	int m;

	init_io();		/* ポート初期化 	*/
	init_LCD();		/* LCD初期化 		*/

	LCD_cursor(0,0);
	LCD_disp("Hello! ULTAKA1");	/* タイトル表示 				*/

	S();				/* brake on (Charge high-side condenser)	*/

	m = mode();			/* mode select 					*/
	LCD_cursor(8,1);
	LCD_disp("READY C");


	while( SW_C == 1 ){
	}
	wait_5sec();

	switch(m){
		case 1:	mode_1();	break;
		case 2:	mode_2();	break;
		case 3:	mode_3();	break;
		case 4:	mode_4();	break;
		case 5:	mode_5();	break;
		case 6:	mode_6();	break;
		case 7:	mode_7();	break;
		case 8:	mode_8();	break;
		case 9:	mode_9();	break;
	}


	while( 1 );
}

void white(void){
	if(AD.DRA > 5000){		/*	右センサ検出	*/
		BS();
		delay(2);
	}
	if(AD.DRB > 5000){		/*	左センサ検出	*/
		SB();
		delay(2);
	}
}



void mode_1(void)
{
	while(1)
	{
		F();
		white();
		delay(5);
	}
}

void mode_2(void)
{
	int i;
	while(1)
	{
		for(i=1;i<=14;i++){
		F();
		white();
		delay(5);
		L();
		white();
		delay(1);
		}

		for(i=1;i<=15;i++){
		F();
		white();
		delay(5);
		}
	}
}

void mode_3(void)
{	
	int i;
	while(1)
	{	
		for(i=1;i<=280;i++)
		{
			F();
			white();
			delay(5);
			SF();
			white();
			delay(5);
		}

		for(i=1;i<=70;i++)
		{	
			L();
			white();
			delay(5);
		}

		for(i=1;i<=250;i++)
		{
			F();
			white();
			delay(5);
		}

		for(i=1;i<=70;i++)
		{
			L();
			white();
			delay(5);
		}

	}
}

void mode_4(void)
{
	int i;
	while(1)
	{
		for(i=1;i<=42;i++){
		F();
		white();
		delay(5);
		L();
		white();
		delay(1);
		}

		for(i=1;i<=15;i++){
		F();
		white();
		delay(5);
		}
	}
}

void mode_5(void)
{
	int i;
	while(1)
	{
		for(i=1;i<=56;i++){
		F();
		white();
		delay(5);
		L();
		white();
		delay(1);
		}

		for(i=1;i<=15;i++){
		F();
		white();
		delay(5);
		}
	}
}

void mode_6(void)
{
	int i;
	while(1)
	{
		for(i=1;i<=14;i++){
		F();
		white();
		delay(5);
		R();
		white();
		delay(1);
		}

		for(i=1;i<=15;i++){
		F();
		white();
		delay(5);
		}
	}
}

void mode_7(void)
{
	int i;
	while(1)
	{
		for(i=1;i<=28;i++){
		F();
		white();
		delay(5);
		R();
		white();
		delay(1);
		}

		for(i=1;i<=15;i++){
		F();
		white();
		delay(5);
		}
	}
}

void mode_8(void)
{
	int i;
	while(1)
	{
		for(i=1;i<=42;i++){
		F();
		white();
		delay(5);
		R();
		white();
		delay(1);
		}

		for(i=1;i<=15;i++){
		F();
		white();
		delay(5);
		}
	}
}

void mode_9(void)
{
	int i;
	while(1)
	{
		for(i=1;i<=56;i++){
		F();
		white();
		delay(5);
		R();
		white();
		delay(1);
		}

		for(i=1;i<=15;i++){
		F();
		white();
		delay(5);
		}
	}
}

void mode_disp(int i)
{
	clr_2();
	LCD_cursor(0,1);
	LCD_disp("MODE:");
	LCD_cursor(5,1);
	switch(i)
	{
		case 1:	LCD_disp("1");	break;
		case 2:	LCD_disp("2");	break;
		case 3:	LCD_disp("3");	break;
		case 4:	LCD_disp("4");	break;
		case 5:	LCD_disp("5");	break;
		case 6:	LCD_disp("6");	break;
		case 7:	LCD_disp("7");	break;
		case 8:	LCD_disp("8");	break;
		case 9:	LCD_disp("9");	break;
	}
}

int mode(void)
{
	int i = 1;
	mode_disp(i);
	while( SW_B == 1 )
	{
		if( SW_A == 0 )
		{
			delay(1);			/* on chattering cancel		*/
			if( SW_A == 0 ) i++;
			if( i >= 10 ) i = 1;
			mode_disp(i);
		}
		while( SW_A == 0 )
		{
		}
		delay(5);				/* off chattering cancel	*/
	}
	return i;
}


void init_io(void)
{
	P4.DDR = 0xff;			/* LCD port output 		*/
	P5.PCR.BYTE = 0xfe;		/* SW A,B,C pull up		*/
	PA.DDR = 0xff;			/* MOTOR port output	     	*/

	AD.CSR.BYTE = 0x39;		/* A/D converter initialize	*/
	
	ITU0.TCR.BYTE = 0x23;		/* Timer internal clock 1/8 	*/
	ITU0.GRA = 2000;		/* 2000  (1kHz)            	*/
	ITU.TSTR.BIT.STR0 = 1;		/* Count start             	*/

}

void F(void)
{
	L_PWM = 0;
	R_PWM = 0;
	L_B = 0;
	L_A = 1;
	R_B = 0;
	R_A = 1;
}

void B(void)
{
	L_PWM = 0;
	R_PWM = 0;
	L_B = 1;
	L_A = 0;
	R_B = 1;
	R_A = 0;
}

void R(void)
{
	L_PWM = 0;
	R_PWM = 0;
	L_B = 0;
	L_A = 1;
	R_B = 1;
	R_A = 0;
}

void L(void)
{
	L_PWM = 0;
	R_PWM = 0;
	L_B = 1;
	L_A = 0;
	R_B = 0;
	R_A = 1;
}

void S(void)
{
	L_PWM = 0;
	R_PWM = 0;
	L_B = 1;
	L_A = 1;
	R_B = 1;
	R_A = 1;
}

void N(void)
{
	L_PWM = 0;
	R_PWM = 0;
	L_B = 0;
	L_A = 0;
	R_B = 0;
	R_A = 0;
}

void SB(void)
{
	L_PWM = 0;
	R_PWM = 0;
	L_B = 1;
	L_A = 1;
	R_B = 1;
	R_A = 0;
}

void BS(void)
{
	L_PWM = 0;
	R_PWM = 0;
	L_B = 1;
	L_A = 0;
	R_B = 1;
	R_A = 1;
}

void FS(void)
{
	L_PWM = 0;
	R_PWM = 0;
	L_B = 0;
	L_A = 1;
	R_B = 1;
	R_A = 1;
}

void SF(void)
{
	L_PWM = 0;
	R_PWM = 0;
	L_B = 1;
	L_A = 1;
	R_B = 0;
	R_A = 1;
}

void wait_1sec(void)			/* wait 1 sec 			*/
{
	int i;

	ITU0.TCR.BYTE = 0x23;		/* Timer internal clock 1/8	*/
	ITU0.GRA = 2000;		/* 2000  (1kHz)             	*/
	ITU.TSTR.BIT.STR0 = 1;		/* Start count              	*/

	for (i = 0; i < 1000; i++)
	{
		while (IMFA0 == 0)
		{
		}
		IMFA0 = 0;
	}
}
		
void wait_5sec(void)			/* 5 sec wait */
{
	clr_1();
	LCD_cursor(0,0);
	LCD_disp("START 5");
	wait_1sec();
	LCD_cursor(6,0);
	LCD_disp("4");
	wait_1sec();
	LCD_cursor(6,0);
	LCD_disp("3");
	wait_1sec();
	LCD_cursor(6,0);
	LCD_disp("2");
	wait_1sec();
	LCD_cursor(6,0);
	LCD_disp("1");
	wait_1sec();
	clr_1();
	LCD_cursor(0,0);
	LCD_disp("GO !");
}

void delay( unsigned int x )				/* x ms delay	*/
{
	unsigned int i;

	i = 0;
	while( i < x )
	{
		if( IMFA0 == 1 )
		{
			IMFA0 = 0;
			i++;
		}
	}
}

void delay450n( void )					/* delay 450ns			*/
{
	unsigned int i;

	for( i = 200 ; i > 0 ; i--);
}

void LCD_cursor(int row, int col)		/* LCDカーソル移動 	*/
{
	LCD_RS = 0;
	LCD_set(0x80 + 0x40 * col + row);
}

void LCD_disp( char *s )			/* LCD文字列表示 	*/
{
	while( *s != 0 )
	{
		LCD_RS = 1;
		LCD_set( *s );
		s++;
	}
}

void LCD_enable( void )				/* LCDイネーブル・パルス 	*/
{
	LCD_E = 1;
	delay450n();
	LCD_E = 0;
	delay450n();
}

void LCD_H(unsigned char data)			/* LCD high data set	 	*/
{
	if ((data & 0x80) == 0) LCD_D7=0; else LCD_D7=1;
	if ((data & 0x40) == 0) LCD_D6=0; else LCD_D6=1;
	if ((data & 0x20) == 0) LCD_D5=0; else LCD_D5=1;
	if ((data & 0x10) == 0) LCD_D4=0; else LCD_D4=1;
	LCD_enable();
} 

void LCD_L(unsigned char data)			/* LCD low data set	 	*/
{
	if ((data & 0x08) == 0) LCD_D7=0; else LCD_D7=1;
	if ((data & 0x04) == 0) LCD_D6=0; else LCD_D6=1;
	if ((data & 0x02) == 0) LCD_D5=0; else LCD_D5=1;
	if ((data & 0x01) == 0) LCD_D4=0; else LCD_D4=1;
	LCD_enable();
} 

void LCD_set( unsigned char data )			/* LCD data set	 	*/
{
	LCD_H( data );
	LCD_L( data );
}

void clr_1(void)					/* LCD LINE 1 CLEAR */
{
	LCD_cursor(0,0);
	LCD_disp("                ");
}

void clr_2(void)					/* LCD LINE 2 CLEAR */
{
	LCD_cursor(0,1);
	LCD_disp("                ");
}

void init_LCD( void )					/* LCD initialize	 	*/
{
	delay( 15 );

	LCD_RS = 0;

	LCD_H( 0x30 );					/* function set 1 		*/
	delay( 5 );
	LCD_H( 0x30 );					/* function set 2 		*/
	delay( 1 );
	LCD_H( 0x30 );					/* function set 3 		*/

	LCD_H( 0x20 );					/* function set 4 bit mode	*/

	LCD_set( 0x28 );					/* function set 4 bit  2 line	*/
	LCD_set( 0x0e );					/* display on 			*/
	LCD_set( 0x06 );					/* entry mode 			*/
	LCD_set( 0x01 );					/* clear display 		*/
	delay( 3 );
}