#include <reg51.h>

#define XFREQ     11000	// crystal freq in kHz

#define RYCHLOST	50
//casy rychlosti v mikrosekundach
//pravy motor
#define R_STOP_TIME			1292 //stoji
#define R_VPRED_MAX_TIME	1100 //rychlo dopredu 
#define R_KROK					1
#define L_KROK					1
#define DIELIK					15	//20 - nevybera 10cm zakrutu

//lavy motor - (upravene)
#define L_STOP_TIME			1260	//stoji 
#define L_VPRED_MAX_TIME   1400 //rychlo dopredu 

//prestavka medzi pulzami
#define REP_PULSE_TIME (18500/2)

//makro - prevedie cas v mikrosekundach na tiky
#define TIME_TO_TICKS(time) ((int)( (time) / 1000.0 * (XFREQ / 12.0) ))

#define REP_PULSE		TIME_TO_TICKS(REP_PULSE_TIME)

//tiky rychlosti
//lavy motor
#define L_STOP			TIME_TO_TICKS(L_STOP_TIME		) //stoji
#define L_VPRED_MAX	TIME_TO_TICKS(L_VPRED_MAX_TIME) //rychlo dopredu 

//pravy motor
#define R_JEMNE		TIME_TO_TICKS(R_JEMNE_TIME		) //jemne vpred
#define R_STOP			TIME_TO_TICKS(R_STOP_TIME		) //stoji
#define R_VPRED_MAX	TIME_TO_TICKS(R_VPRED_MAX_TIME) //rychlo dopredu 

sbit SERVO_LEFT  = P3^1;     // servo output
sbit SERVO_RIGHT = P3^0;     // servo output

sbit SWITCH_UP   = P1^2; // tlacitka
sbit SWITCH_DOWN = P1^3; 

sbit S1 = P3^3; // pravy vonkajsi senzor
sbit S2 = P3^2; // 
sbit S3 = P3^5; // 
sbit S4 = P3^4; // lavy vonkajsi senzor
sbit S5 = P3^6; // predny senzor 

sbit PVOS = P3^3; // pravy vonkajsi senzor
sbit PVNS = P3^2; // 
sbit LVNS = P3^5; // 
sbit LVOS = P3^4; // lavy vonkajsi senzor
sbit PRS = P3^6; // predny senzor 

sbit D1 = P1^4;//dioda 4. od kondenzatora
sbit D2 = P1^5;
sbit D3 = P1^6;
sbit D4 = P1^7;
sbit D5 = P3^7;//dioda uplne 5. od kondenzatora

bit servo_pulse = 0;
bit servo_which = 0;//0=left 1=right
unsigned int servo_left_tick_count = 0;
unsigned int servo_right_tick_count = 0;
void it_timer0(void) interrupt 1 using 1
{
	SERVO_LEFT = 0;
	SERVO_RIGHT = 0;

	TR0 = 0;    // timer 0 off
	if(servo_pulse)
	{
		servo_pulse = 0;

   	TL0 = (0x0 - REP_PULSE) & 0xFF;
   	TH0 = (0x0 - REP_PULSE) >> 8;
		TR0 = 1;    // timer 0 on   
   }
   else
   {
		servo_pulse = 1;

		if(servo_which==0)//left servo
		{
   		TL0 = (0x0 - servo_left_tick_count) & 0xFF;
   		TH0 = (0x0 - servo_left_tick_count) >> 8;
			SERVO_LEFT = 1;
			servo_which=1;
		}
		else 					//right servo
		{
   		TL0 = (0x0 - servo_right_tick_count) & 0xFF;
   		TH0 = (0x0 - servo_right_tick_count) >> 8;
			SERVO_RIGHT = 1;
			servo_which=0;
		}
		
		TR0 = 1;    // timer 0 on   
	}
	D1=S1;
	D2=S2;
	D3=S3;
	D4=S4;
	D5=S5;
}

void cakaj(int cas)
{
	int i;
	unsigned int x;

	for(i=0;i<cas;i++)
		for(x=0;x<1000;x++);
}
void diodax(int dioda,bit x)
{
		switch(dioda)
		{
			case 0:D1=x;break;
			case 1:D2=x;break;
			case 2:D3=x;break;
			case 3:D4=x;break;
			case 4:D5=x;break;
		}
}
void main(void)
{   
	int i;
	int zamrznutie=0;
	int right_counter=0;
	int left_counter=0;

   zaciatok:

	for(i=0;i<5;i++)
	{
		diodax(i,0);
		cakaj(25);
		diodax(i,1);
		cakaj(25);
	}

	servo_left_tick_count = L_STOP;
	servo_right_tick_count = R_STOP;

	servo_pulse = 0;
   TMOD = 0x01; // 16 bit timer0  
   TL0 = (0x0 - REP_PULSE) & 0xFF;
   TH0 = (0x0 - REP_PULSE) >> 8;

	ET0 = 1;	// enable timer 0	
	EA = 1;  // enable interrupts
	TR0 = 1; // timer 0 on   
	
   D1=D2=D3=D4=D5=1;
	servo_right_tick_count = R_STOP;
	servo_left_tick_count = L_STOP;

	while(!SWITCH_UP);
	while(SWITCH_UP);

	servo_right_tick_count = R_VPRED_MAX;
	servo_left_tick_count = L_VPRED_MAX;
	while(1)
	{

		if(SWITCH_DOWN)goto zaciatok;
		if(PRS)
		{
			servo_right_tick_count = R_VPRED_MAX;
			servo_left_tick_count = L_VPRED_MAX;
			continue;
		}
		
		if(PVOS)
		{
			right_counter--;
			if(right_counter<=-DIELIK)
			{
				right_counter=0;
				if(servo_right_tick_count<R_STOP)
					servo_right_tick_count+=R_KROK;
			}
		}
		else
		{
			right_counter++;
			if(right_counter>=DIELIK)
			{
				right_counter=0;
				if(servo_right_tick_count>R_VPRED_MAX)
					servo_right_tick_count-=R_KROK;
			}
		}
		
		if(LVOS)
		{
			left_counter--;
			if(left_counter<=-DIELIK)
			{
				left_counter=0;
				if(servo_left_tick_count>L_STOP)
					servo_left_tick_count-=L_KROK;
			}
		}
		else			
   	{
			left_counter++;
			if(left_counter>=DIELIK)
			{
				left_counter=0;
	   		if(servo_left_tick_count<L_VPRED_MAX)
	   			servo_left_tick_count+=L_KROK;
	   	}
   	}

		if(PVOS&&LVOS)
		{
			zamrznutie++;
			if(zamrznutie>100)
			{
				zamrznutie=0;
				servo_left_tick_count=L_VPRED_MAX;
				servo_right_tick_count=R_STOP;
				while(PVOS&&LVOS);
			}
		}
	}
}