//teachrobo.c			Dec.2007 (12/26)
// PIC C compiler PCM
// RC Oscillator; 5 servos
// infrared ray TV remote control analyzer(resolver) robot
// Capture mode shift register receiver
// 7 channel 4*8=32 bit/channel 
// Author Toshio Murayama
#include  <16f819.h>
#fuses  INTRC_IO,NOWDT,NOPROTECT,PUT,NOLVP,CCPB3,NOBROWNOUT		//2F30
#use delay(CLOCK = 8000000)	//8MHz
	setup_osillator(OSC_8MHZ);
#use fast_io(A) 
#use fast_io(B)
static long t1,t2,width;
static int pt=0,sw;
static int rx[4],code[7][4];
//switch;1=forward,2=back,3=right turn,4=left turn,5=stop,6=round right,7=round left
/********************************/
static int mia,mik,hia,hik,mia1,mia2,mik1,mik2,hia1,hia2,hik1,hik2,atama;
static int srv1,srv2,srv4,srv8,srvaa;
/********************************/
#int_ccp1
void pulse()
{
	if(pt>24)		//8*3byte=24bit
		return;
	t2=ccp_1;
	if(t2>t1)
		width=t2-t1;
	else
		width=t2+~t1;	//+1
	if(width>1600)		//800us/0.5=1600<noise 
	{
		t1=t2;
		if(width<2800)		//1400us/0.5=2800;500~1000|600~1200 
		{
			shift_left(rx,4,1);
			pt++;
		}
		else if(width<5200)	//2600us/0.5=5200;~2000|~2400
		{
			shift_left(rx,4,0);
			shift_left(rx,4,1);
			pt+=2;
		}
		else if(width<7600)	//3800us/0.5=7600;~3000|~3600
		{
			shift_left(rx,4,0);
			shift_left(rx,4,0);
			shift_left(rx,4,1);
			pt+=3;
		}
		else			//~4000|~4800 over
		{
			shift_left(rx,4,0);
			shift_left(rx,4,0);
			shift_left(rx,4,0);
			shift_left(rx,4,1);
			pt+=4;
		}
	}
}
/******************************/
void servo();
void srv2m(int pp,int qq);
void srv4p(int pp,int qq);
void tyuuritu();
void asihumi(int ee);
void mae(int ee);
void usiro(int ee);
void migimae(int ee);
void hidarimae(int ee);
/******************************/
void space()
{		//**1	find long space 5ms over:1.2ms*4bit=4.8ms
	int aa,bb;
	do
	{
		bb=0;
		while(input(PIN_B3))	//count during sensor H level
		{
			bb++;
			if(bb>250)
				bb=250;
			sw=input_A()>>1;
			output_B(sw<<5);
			delay_us(16);	//1 count=20us
		}
	/* L level check neglect under 400us */
		aa=0;
		while(!input(PIN_B3))	//count during sensor L level
		{
			aa++;
			sw=input_A()>>1;
			output_B(sw<<5);
			delay_us(16);	//1 count=20us
		}
		if(aa<20)
			bb=0;
	}while(bb<250);		//H level 5ms over exit
	t1=ccp_1;
	pt=0;
}
/******************************/
int main(void)
{
	int  aa,bb,dd,ee;
//	#asm	// setup_osillator(OSC_8MHZ);
//		movlw 0x74
//		movwf 0x8F	//OSCCON
//	#endasm
	set_tris_a(0x1F);	//port a1,2,3 switch, a4 push button
	set_tris_b(0x08);	//port b3 ccp IR input  decision teaching
	setup_adc_ports(RA0_ANALOG);	//A0 analog input, Ref=Vdd;A1,2,3,4,5 digital I/O
	setup_adc(ADC_CLOCK_DIV_32);
	setup_ccp1(CCP_CAPTURE_FE);	//capture fall; active low
	setup_timer_1(T1_INTERNAL);	//0.2us/count,1us=5count
	enable_interrupts(INT_CCP1);
	enable_interrupts(GLOBAL);
	output_B(0);
	hia=read_eeprom(0);
	if(hia==0xFF) hia=0x50;
	hik=read_eeprom(1);
	if(hik==0xFF) hik=0x50;
	mik=read_eeprom(2);
	if(mik==0xFF) mik=0x50;
	mia=read_eeprom(3);
	if(mia==0xFF) mia=0x50;
	atama=read_eeprom(4);
	if(atama==0xFF) atama=0x50;
	mia1=mia- 6;
	mia2=mia+ 6;
	hia1=hia- 6;
	hia2=hia+ 6;
	mik1=mik-8;
	mik2=mik+8;
	hik1=hik-8;
	hik2=hik+8;
	srv1=hia;
	srv2=hik;
	srv4=mik;
	srv8=mia;
	srvaa=atama;
	aa=5;		//eeprom read
	for(dd=0;dd<7;dd++)
	{
		for(bb=0;bb<4;bb++)
		{
			code[dd][bb]=read_eeprom(aa);	//EEPROMを読み出す
			aa++;
		}
	}
	for(aa=0;aa<50;aa++)
		servo();
	if(input_A()>15)	
		tyuuritu();
	space();
	if(sw>7)		//push button on; teaching start
	{
//***1			//teaching
		do
		{
			do	//digital switch setting key no.1~7 endkey=0 
			{
				space();
				if(sw==0)
					break;	//スイッチが0で割付を終了させる
				if(sw<8)
				{
					dd=sw;
					sw--;		//memo address= input data-1
					for(bb=0;bb<4;bb++)
						rx[bb]=0;
					shift_left(rx,4,1);
					delay_ms(30);		//recive time 1.2ms*8*3byte=28.8ms
					for(bb=0;bb<4;bb++)
						code[sw][bb]=rx[bb];
					for(aa=0;aa<2;aa++)
					{
						output_B(dd<<5);
						delay_ms(200);
						output_B(0);
						delay_ms(200);
					}
				}
			}while(1);
//***2
			do		//check remocon and LED display
			{
				space();
				if(sw>7)
					break;	//to write_eeprom or restart( push button on)
				for(aa=0;aa<4;aa++)
					rx[aa]=0;
				shift_left(rx,4,1);
				delay_ms(30);		//reciving time 1.2ms*3*8=28.8ms
				dd=0;
				do
				{
 					for(bb=0;bb<4;bb++)
					{
						if(code[dd][bb]!=rx[bb])
						{
							dd++;
							break;
						}
					}
					if(bb==4)
					{
						for(aa=0;aa<2;aa++)
						{
							output_B((dd+1)<<5);
							delay_ms(200);
							output_B(0);
							delay_ms(200);
						}
						break;
					}
				}while(dd<7);
			}while(1);
			if(sw==8)
				break;	//スイッチが0で割付を終了させる
		}while(1);	//if sw>7 then restart;push button on
//***3
		aa=5;
		for(dd=0;dd<7;dd++)
		{
			output_B((dd+1)<<5);
			for(bb=0;bb<4;bb++)
			{
				ee=code[dd][bb];
				write_eeprom(aa,ee);	//結果をEEPROMに書き込む
				aa++;
			}
			delay_ms(300);
		}
		for(bb=0;bb<2;bb++)
		{
			output_B(0xE0);
			delay_ms(300);
			output_B(0);
			delay_ms(100);
		}			//eeprom write end
	}
//**4
	for(aa=0;aa<50;aa++)
	{
		servo();
		delay_ms(20);
	}
	do		//read remocon push button and drive servo moter
	{
		space();
		for(aa=0;aa<4;aa++)
			rx[aa]=0;
		shift_left(rx,4,1);
		delay_ms(30);		//recive time 1.2ms*3*8=28.8ms
		dd=0;
		do
		{
			for(bb=0;bb<4;bb++)
			{
				if(code[dd][bb]!=rx[bb])
				{
					dd++;
					break;
				}
			}
			if(bb==4)
			{
				switch (dd)
				{
					case 0:mae(2);break;
					case 1:usiro(2);break;
					case 2:hidarimae(2);break;
					case 3:migimae(2);break;
					case 4:asihumi(2);break;
					case 5:
					{
						if(srvaa==atama)
						{
							for(srvaa=atama;srvaa<(atama+20);srvaa++)
								servo();
						}
						else
						{
							while(srvaa>atama)
							{
									srvaa--;
									servo();
							}
						}
						break;
					}
					case 6:
					{
						if(srvaa==atama)
						{
							 for(srvaa=atama;srvaa>(atama-20);srvaa--)
								servo();
						}
						else
						{
							while(srvaa<atama)
							{
									srvaa++;
									servo();
							}
						}
						break;
					}
				}
				break;
			}
		}while(dd<7);
	}while(1);
}

/******************************/
void servo()
{
	int bb;	/*100=1.4ms*/
	output_high(PIN_A7);		//check pulse
	output_high(PIN_B0);
	for(bb=0;bb<srv1;bb++)
		delay_us(16); 
	output_low(PIN_B0);
/****************/
	output_high(PIN_B1);
	for(bb=0;bb<srv2;bb++)
		delay_us(16); 
	output_low(PIN_B1);
/****************/
	output_high(PIN_B2);
	for(bb=0;bb<srv4;bb++)
		delay_us(16); 
	output_low(PIN_B2);
/****************/
	output_high(PIN_B4);
	for(bb=0;bb<srv8;bb++)
		delay_us(16); 
	output_low(PIN_B4);
/****************/
	output_high(PIN_A6);
	for(bb=0;bb<srvaa;bb++)
		delay_us(16); 
	output_low(PIN_A6);
	output_low(PIN_A7);
	delay_ms(8); 			//ms(10)=repeat time=15ms
}
/******************************/
void tyuuritu()
{
	int aa,bb;
	do{
		set_adc_channel(0);		//port A0:analog input
		delay_us(50);
		aa =(read_adc()>>1)+25;	//0~127+25
		bb=input_A()>>1;
		switch (bb)
		{
			case 8:				//switch=0
			{
				write_eeprom(0,hia);
				write_eeprom(1,hik);
				write_eeprom(2,mik);
				write_eeprom(3,mia);
				write_eeprom(4,atama);
				OUTPUT_B(0);
				return;
			}
			case 9:			//switch=1
			{	hia=aa;
				OUTPUT_B(0x20);
				break;
			}
			case 10:			//switch=2
			{	hik=aa;
				OUTPUT_B(0x40);
				break;
			}
			case 11:			//switch=3
			{	mik=aa;
				OUTPUT_B(0x60);
				break;
			}
			case 12:			//switch=4
			{	mia=aa;
				OUTPUT_B(0x80);
				break;
			}
			case 13:			//switch=5
			{	atama=aa;
				OUTPUT_B(0xA0);
				break;
			}
	//		default:
	//			break;
		}
		mia1=mia- 6;
		mia2=mia+ 6;
		hia1=hia- 6;
		hia2=hia+ 6;
		mik1=mik-8;
		mik2=mik+8;
		hik1=hik-8;
		hik2=hik+8;
		srv1=hia;
		srv2=hik;
		srv4=mik;
		srv8=mia;
		srvaa=atama;
		for(aa=0;aa<5;aa++)
			servo();
	}while( true);
}
/******************************/
void  srv2m(int pp,int qq)
{int bb;
 srv4-=2;
 servo();
 for(srv2=pp;srv2>qq;srv2--)
 {
	srv4--;
	servo();
 }
}
/******************************/
void srv4p(int pp,int qq)
{int bb;
 srv2+=2;
 servo();
  for(srv4=pp;srv4<qq;srv4++)
  {
    srv2++;
    servo();
  }
}
/******************************/
void asihumi(int ee)
{int bb,dd;
 srv1=hia;
 srv2=hik;
 srv8=mia;
 srv2m(hik,hik1);
 for(dd=0;dd<10;dd++)
	servo();
 for (bb=0;bb<ee;bb++)
 {
  srv4p(mik1,mik2);
 for(dd=0;dd<10;dd++)
	servo();
  srv2m(hik2,hik1);
 for(dd=0;dd<10;dd++)
	servo();
 }
 srv4p(mik1,mik);
 srv2=hik;
 for(dd=0;dd<10;dd++)
	servo();
}
/******************************/
void mae(int ee)
{int bb,dd;
	srv1=hia;
	srv2=hik;
	srv8=mia;
	srv2m(hik,hik1);
	for(dd=0;dd<5;dd++)
		servo();
	srv4+=2;
	for(srv8=mia;srv8<mia2;srv8++)
	{	servo();
		srv1++;
	}
	for(bb=0;bb<ee;bb++)
	{
		srv4p(mik1,mik2);
		for(srv8=mia2;srv8>mia1;srv8--)
		{
			servo();
			srv1--;
		}
		srv2m(hik2,hik1);
		for(srv8=mia1;srv8<mia2;srv8++)
		{
			servo();
			srv1++;
		}
	}
	srv4p(mik1,mik);
	for(srv8=mia2;srv8<mia;srv8--)
	{
		servo();
		srv1--;
	}
	srv1=hia;
	srv2=hik;
	srv4=mik;
	for(dd=0;dd<5;dd++)
		servo();
}
/******************************/
void usiro(int ee)
{int bb,dd;
	srv1=hia;
	srv2=hik;
	srv4=mik;
	srv2m(hik,hik1);
	for(srv8=mia;srv8>mia1;srv8--)
	{ servo();
	   srv1--;
	}
	for (bb=0;bb<ee;bb++)
	{
	  srv4p(mik1,mik2);
 	 for(srv8=mia1;srv8<mia2;srv8++)
 	 {servo();
  	  srv1++;
	 }
	 srv2m(hik2,hik1);
	 for(srv8=mia2;srv8>mia1;srv8--)
	 {servo();
	  srv1--;
	 }
	}
	srv4p(mik1,mik);
	for(srv8=mia1;srv8<mia;srv8++)
	{ servo();
	  srv1++;
	}
	srv2=hik;
	srv1=hia;
	for(dd=0;dd<5;dd++)
		servo();
}
/******************************/
void migimae(int ee)
{int bb,dd;
 srv2=hik;
 srv4p(mik,mik2);
 for(srv1=hia;srv1>hia1;srv1--)
   servo();
 for (bb=0;bb<ee;bb++)
 {
	 srv2m(hik2,hik1);
	 for(srv1=hia1;srv1<hia2;srv1++)
   		servo();
 	 srv4p(mik1,mik2);
 	for(srv1=hia2;srv1>hia1;srv1--)
   		servo();
 }
 srv1=hia;
 srv2m(hik2,hik);
 for(dd=0;dd<5;dd++)
	servo();
}
/******************************/
void hidarimae(int ee)
{int bb,dd;
 srv2=hik;
 srv2m(hik,hik1);
 for(srv8=mia;srv8<mia2;srv8++)
   servo();
 for (bb=0;bb<ee;bb++)
 {
   srv4p(mik1,mik2);
 	for(srv8=mia2;srv8>mia1;srv8--)
   		servo();
  	srv2m(hik2,hik1);
 	for(srv8=mia1;srv8<mia2;srv8++)
   		servo();
 }
 srv8=mia;
 srv4p(mik1,mik);
 for(srv8=mia2;srv8>mia;srv8--)
   servo();
 srv2=hik;
 for(dd=0;dd<5;dd++)
	servo();
}