#include <89c51rd2.h>
#include <stdio.h>

//************* Definicao dos sinais de hardware *****************
// Enable da placa de sensores		P1.2		Out		Activo a 0
// Clock da placa de sensores			P1.0		In			Activo a 0
// Dados da placa de sensores			P1.1		In			(S6 .. S0)
//////////////////////////////////////////////////////////////////
// Ligar sonar								P1.3		Out		Activo a 1
// Eco do sonar							P3.3		In			Activo a 1
//////////////////////////////////////////////////////////////////
// Direccao do motor 1 (esquerdo)	P3.4		Out		(1-frente 0-tras)
// Direccao do motor 2 (direito)		P3.5		Out		(1-frente 0-tras)
// PWM motor 1 (esquerdo)				P1.4		Out
// PWM motor 2 (direito)				P1.5		Out
//************* Placa com IVs ************************************
//
//              TRAS
//			X X X D4 D3 D2 X X		Leitura da Placa de IV
//             FRENTE
//
//        TRAS
//       0 0 0	(0x00)	sobre a linha					estado=0
//       0 0 1	(0x04)	fugiu para a esquerda		estado=1
//       0 1 1 (0x0C)	fugiu para a esquerda		estado=2
//       1 1 1 (0x1C)	fora da linha					estado=3
//       1 0 0	(0x10)	fugiu para a direita			estado=4
//       1 1 0	(0x18)	fugiu para a direita			estado=5
//       FRENTE
//****************************************************************

sbit enable_sensores = P1^2;	//Placa de sensores de fita
sbit clock_sensores = P1^0;
sbit data_sensores = P1^1;

sbit dir0 = P3^4;					//Motores
sbit dir1 = P3^5;

bit flag1 = 1;
bit flag10ms = 0;

unsigned char Leitura_IV;
unsigned char cont1;
unsigned char cont10ms = 10;
unsigned char estado = 0;

const Vm = 80;
const k = 20;
const k1 = 30;
const k2 = 50;

#define DIR(a,b) dir0=a; dir1=b								// DIR(esquerdo,direito)
#define SET_PWM(l,r) CCAP1H=(255-r); CCAP2H=(255-l)	// SET_PWM(esquerdo,direito)


//************* Interrupt do T/C0 ********************************
void int_tc0 (void) interrupt 1	using 2
{
	TH0 = 0xf9;						// T/C1 para gerar overflow em 1mS
	TL0 = 0x7E;
	cont10ms--;						// Contador para 10mS
	if(cont10ms == 0)
	{
		cont10ms = 10;			  	// Mudar este valor para alterar a frequência de controlo
		flag10ms = 1;
	}
	cont1--;							// Contador de mS
	if(cont1 == 0) flag1=1;	
}

//************* Inicializacao do uC ******************************
void inic(void)
{
	CKCON = 0x2f;					// Clock dobrado, perifericos com 12 ciclos
	
	RCAP2H = 0xff;					// T/C2 para gerar baud rate de 9600 bps
	RCAP2L = 0xbf;
	T2CON = 0x34;
	SCON = 0x52;					// UART em modo 1

	CMOD = 0; 						// PCA configuration 
  	CCON = 0x40;
 	CCAPM1 = 0x42;					// Configuração dos módulos para PWM 
  	CCAPM2 = 0x42;

	TMOD = 0x91;					// T/Cs em modo de 16 bits, T/C1 GATE on (para sonar)
	TH0 = 0xf9;						// T/C0 para gerar overflow em 1mS
	TL0 = 0x7E;

	IPL = 0x02;						// Int do T/C0 com maior prioridade
	IE = 0x82;						// Enable da int do T/C0

  	TR0 = 1;							// T/C0 on	
}

//************* Leitura dos detectores de fita *******************
unsigned char ler_IV(void)
{
	unsigned char i, leitura;

	leitura=0;	
	enable_sensores=0;
	for (i=1; i<8; i++)
	{
		leitura=leitura<<1;
		while(clock_sensores==1);
		if (data_sensores==1) leitura++;
		while(clock_sensores==0);
	}
	enable_sensores=1;
	return leitura;
}

//************* PROGRAMA PRINCIPAL *******************************
void main(void)
{
	inic();	
	DIR(1,1);
	SET_PWM(Vm,Vm);
	while(1)
	{
		while(flag10ms == 0);		// Espera inicio de ciclo de controlo
		flag10ms = 0;
		Leitura_IV = ler_IV();
		Leitura_IV = Leitura_IV & 0x1C;
		
		switch (Leitura_IV)
		{
			case 0x0:					// Centrado na linha (estado 0)
				break;
				
			case 0x10:					// Sai pela direita (estado 4)
				SET_PWM(Vm-k,Vm+k);
				break;
				
			case 0x18:					// Sai pela direita + (estado 5)
				SET_PWM(Vm-k1,Vm+k1);
				break;
				
			case 0x04:					// Sai pela esquerda (estado 1)
				SET_PWM(Vm+k,Vm-k);
				break;
				
			case 0x0C:					// Sai pela esquerda + (estado 2)
				SET_PWM(Vm+k1,Vm-k1);
				break;
				
			case 0x1C:					// Fora da linha (estado 3)
				SET_PWM(0,0);
		}
		
	}
}