/*  MarkGuiver 1.0 - Mark-III implementation  
    Copyright (C) 2008 Kabute & Kaderno & Rodland

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/

#define PIC_XTAL 20MHz

#include	<pic.h>
__CONFIG(HS&UNPROTECT&PWRTEN&LVPDIS&WDTDIS&BOREN);
//**************************************************************************
//	Definición de constantes
//**************************************************************************
#define	Delay_us(x)	{ unsigned char _dcnt; \
			  _dcnt = (x)*((20)/(12)); \
			  while(--_dcnt != 0) continue; }

#define FALSE   0
#define TRUE    1

#define EYE_R	0b010000
#define EYE_L	0b011000
#define LINE_R	0b00101000 // 101000
#define LINE_C	0b00110000 // 110000
#define LINE_L	0b00111000 // 111000

#define OFFSETR	0
#define OFFSETL 0
//**************************************************************************
//	Definición de variables
//**************************************************************************
union INTVAL
{
int i;
char b[2];
};
//**************************************************************************
//	Variables
//**************************************************************************
bit segundo;
unsigned char Reset;
unsigned char  i, cont;
unsigned int lineL, lineC, lineR, media;
int speedL, speedR;
int eyeL, eyeR, k, j;
unsigned long  lineAvg;
union INTVAL Temp, Total;
const char *Mensaje="\r\nMARK III.\r\n";
//**************************************************************************
//	Gestion de interrupciones
//**************************************************************************
static void interrupt isr(void)						//Aqui estan las funciones de interrupcion 
{
	if(TMR1IF)
	{
		if(RB2)
		{
			RB2=0;
			RB1=1;
			Temp.i=speedR;
			Total.i +=Temp.i;
		}
		else if(RB1)
		{
			RB2=0;
			RB1=0;
			Total.i=0x30D4 -Total.i;	//20ms=12500(0x20D4) pulsos
			Temp.i=Total.i;
		}
		else if(!RB1 & !RB2)
		{
			RB2=1;
			Temp.i=speedL;
			Total.i =Temp.i;
		}
		else;
		Temp.i = 0xFFFF - Temp.i;	//Hacemos el complementario.
		TMR1L=Temp.b[0];
		TMR1H=Temp.b[1];
		TMR1IF=0;
		if(cont++==5)
		{
			segundo=1;
			cont=0;
		}
		else;
	}
	else;
}
//***************************************************************************
//	Funciones
//***************************************************************************
void Delay_ms(unsigned char milliseg)
{
	while(milliseg--)
	{
		Delay_us(250);Delay_us(250);
		Delay_us(250);Delay_us(250);
	}
}
void Delay_seg(unsigned char segundos)
{
	while(!segundo);
	segundo=0;
	while(segundos--)
	{
		while(!segundo);
		segundo=0;
	}
}
//Envia un byte por el puerto serie
void putchar(unsigned char c)
{
    while(TXIF==0);
    TXREG=c;
}
// Lee un caracter por el puerto serie
unsigned char getchar(void)
{
	unsigned char c;
  
	while(RCIF==0);
	c=RCREG;
	return c;
}
//Envia una cadena de texto por el puerto serie
void putstring(const char *s)
{
    unsigned char i=0;

    if(TXEN==0) TXEN=1;
    while(s[i]!=0)
	{
    	while(TXIF==0);
        TXREG=s[i];
        i++;
    }
}
//Convierte un numero en una cadena de texto y envia por el puerto serie
void putnum(unsigned int num )
{
	unsigned char buf[5];
   	int i = 0;
	unsigned char c;

	c=(unsigned char)(num % 10);
	c+='0';
	buf[i] = c;
    i++;
    num /= 10;
	while (num != 0)
	{
		c=(unsigned char)(num % 10);
		c+='0';
		buf[ i ] = c;
      	i++;
      	num /= 10;
	}
	while ( i > 0 )
   	{
    	i--;
		putchar(buf[i]);
   	}
}
// Escribe un dato en la memoria EEPROM en las direccion address
void write_EE(unsigned char address, int data)
{
	while(WR); 			// espera hasta que termina de escribir
	EEADR = address; 	// guardo la direccion
	EEDATA = data;		// Cargo el dato que se va a escribir
	EEPGD = 0;			// 
	WREN = 1; 			// Activo escritura
	GIE = 0;			// Desactivo interrupciones
	EECON2 = 0x55;		// Requiere un ciclo de escritura
	EECON2 = 0xaa; 		// "
	WR = 1;				// Escribo
	while(WR);			// Espera hasta que termina de escribir
	GIE = 1;			// Reactivo interrupciones
	WREN = 0;			// Desactivo escritura
}
// Lee un valor integer como dos bytes de la EEPROM en la direccion address
unsigned char read_EE(unsigned char address)
{	
	unsigned char data;
	
	EEADR = address;	// Guardo la direccion
	EEPGD = 0;			// 
	RD = 1;				// Leo
	data = EEDATA;		// Guardo el dato leido
	return data;		//Devuelvo el dato
}

void InitReset(void)
{
	Reset=0x01;
	write_EE(0xFF, Reset);
}
//Lectura de puerto analógico (cambiamos de char a int HOYGAN!)
unsigned int a2d(unsigned char pin)
{
	// seleccionar pin
	ADCON0 = 0b10000001 | pin;
	//espera
	Delay_us(100);
	// inicio de conversion
	ADCON0 |= 0x04;
	// espera hasta completar
	while (ADCON0 & 0x04);
	return(ADRESH*256+ADRESL);
}
//Control del MotorR <motor derecho> (direccion <0 avance,1 retroceso>, velocidad <entre 0 y 100>)
void MotorR(unsigned char dirR, int velR )
{
	if(dirR)
		speedR = 860 - (velR*2);
	else
		speedR = 860 + (velR*2);
}
//Control del MotorL <motor izquierdo> (direccion <0 avance,1 retroceso>, velocidad <entre 0 y 100>)
void MotorL(unsigned char dirL, int velL )
{
	if(dirL) 
		speedL = 845 + (velL*2);
	else
		speedL = 845 - (velL*2);
}
void test_motores(void)
{
	unsigned char iter=0;

	for (iter = 0; iter <= 100; iter+=10)	/* Avanza aumentando la velocidad */
	{ MotorR(0,iter); MotorL(0,iter); Delay_seg(3);}  /* Ambos motores avanzan */
    for (iter = 0; iter <= 2; iter++)
	{ MotorR(0,100); MotorL(0,100); Delay_seg(3);  } /* +Max */
	for (iter = 0; iter <= 2; iter++)
	{ MotorR(0,0); MotorL(0,0); Delay_seg(3); }     /* Stop */
	for (iter = 0; iter <= 100; iter+=10)	/* Retrocede aumentando la velocidad */
	{ MotorR(1,iter); MotorL(1,iter); Delay_seg(3); }
	for (iter = 0; iter <= 2; iter++) { MotorR(1,100); MotorL(1,100); Delay_seg(3); } /* -Max */
	for (iter = 0; iter <= 2; iter++) { MotorR(0,0); MotorL(0,0); Delay_seg(3); }     /* Stop */
	for (iter = 0; iter <= 100; iter+=10)	/* gira sobre si mismo aumentando la velocidad */
	{ MotorR(0,iter); MotorL(1,iter); Delay_seg(3); } /* Motor izq avanza, motor dcho retrocede */
	for (iter = 0; iter <= 2; iter++) { MotorR(0,100); MotorL(1,100); Delay_seg(3); } /* +-Max */
	for (iter = 0; iter <= 100; iter+=10)	/* Cambio de direcciones mientras gira */
	{ MotorR(1,iter); MotorL(0,iter); Delay_seg(3); }
	for (iter = 0; iter <= 2; iter++) { MotorR(1,100); MotorL(0,100); Delay_seg(3); } /* -+Max */
	for (iter = 0; iter <= 2; iter++) { MotorR(0,0); MotorL(0,0); Delay_seg(3); }     /* Stop */
}
void test_infrarrojos(void)
{
	putstring("Right:");	putnum(lineR); putchar(';');
	putstring("Center:"); putnum(lineC);putchar(';');
	putstring("Left:"); putnum(lineL);	putchar(';');
	putchar('\r');
	lineR = a2d(LINE_R);//Lectura sensor optico derecho <0 luz, 255 oscuridad>
	lineC = a2d(LINE_C);//Lectura sensor optico central <0 luz, 255 oscuridad>
	lineL = a2d(LINE_L);//Lectura sensor optico izquierdo <0 luz, 255 oscuridad>
	if (lineC>media)
	{
		if(lineL>media)
		{
		//	MotorL(0,0);MotorR(0,30);
			Delay_seg(2);
		}
		else
		{
			if(lineR>media)
			{
		//		MotorL(0,30);MotorR(0,0);
				Delay_seg(2);
			}
			else
			{
		//		MotorL(0,20);MotorR(0,20);
				Delay_seg(1);
			}
		}
	}
	else
	{
		if(lineL>media)
		{
		//	MotorL(0,0);MotorR(0,30);
			Delay_seg(2);
		}
		else
		{
			if(lineR>media)
			{
			//	MotorL(0,30);MotorR(0,0);
				Delay_seg(2);
			}
			else;
		}
	}
}


void poltergeist(void)
{
	int CERCA=350;	
	int MUYCERCA=450;
	int MUYLEJOS=10;
	int luz = 700; // 200 si hay luz ambiente y sobre 750 en la oscuridad
//lectura sensores 
	lineR = a2d(LINE_R);
	lineL = a2d(LINE_L);



if ((lineR>luz) && (lineL>luz)){ // mientras no ve la luz giramos sobre si mismo

	MotorL(0,30);
    MotorR(1,30);
	//Delay_seg(10);
} // aquí estamos mirando hacia la luz





else
{
/*
//Corregimos la trayectoria
if ((lineR>lineL))
{
 if((lineR-lineL)>70)
  {
		MotorR(0,30);
		MotorL(1,30);
  }
}

else
{
 if((lineL-lineR)>70)
  {
		MotorR(1,30);
		MotorL(0,30);
   }
}

*/

//Hacia delante hasta encontrar algo
while(a2d(EYE_R)<CERCA && a2d(EYE_L)<CERCA){
		MotorR(1,100);
		MotorL(1,100);

 if( (lineR>luz) || (lineL>luz)) break;
			}
// Encontramos un obstáculo e xiramos hacia o lado que hai máis luz

if (lineR>=lineL)
{
		MotorR(1,100);
		MotorL(0,100);
		Delay_seg(10);
       if(a2d(EYE_R)>=CERCA && a2d(EYE_L)>=CERCA){
		MotorR(1,100);
		MotorL(1,100);
		Delay_seg(10);
		MotorR(0,100);
		MotorL(1,100);
		Delay_seg(10);
}

else {

		MotorR(1,100);
		MotorL(0,100);
		Delay_seg(20);
}


}

else 
{
	MotorR(0,100);
	MotorL(1,100);
	Delay_seg(10);

       if(a2d(EYE_R)>=CERCA && a2d(EYE_L)>=CERCA){
		MotorR(1,100);
		MotorL(1,100);
		Delay_seg(10);
	    MotorR(1,100);
		MotorL(0,100);
		Delay_seg(10);
}

  else
{
	MotorR(1,100);
	MotorL(0,100);
	Delay_seg(20);
}

}


}
}

void testeyes(void)
{
	eyeR = a2d(EYE_R);
	eyeL = a2d(EYE_L);
	putstring("Right eye:"); putnum(eyeR); putchar(';');
	putstring(" Left eye:"); putnum(eyeL);putchar(';');
	putchar('\r');
} 

void followTheLeader(void){
	int center = 0,ncenter=0;
	while(1){
		lineR = a2d(LINE_R);//Lectura sensor optico derecho <0 luz, 255 oscuridad>
		lineC = a2d(LINE_C);//Lectura sensor optico central <0 luz, 255 oscuridad>
		lineL = a2d(LINE_L);//Lectura sensor optico izquierdo <0 luz, 255 oscuridad>

		putstring("Right:");	putnum(lineR); putchar(';');
		putstring("Center:"); putnum(lineC);putchar(';');
		putstring("Left:"); putnum(lineL);	putchar(';');
		putchar('\r');

		if (lineC > 250 || ncenter > 10 || ncenter<-10){
			ncenter =0;
		}

		if(lineR > 250 && lineL > 250){
			MotorR(1,100);
			MotorL(1,100);
		}else if(lineR > 250 || ncenter > 0){
			ncenter++;
			MotorR(0,40);
			MotorL(1,100);
		}else if(lineL > 250 || ncenter < 0 ){
			ncenter--;
			MotorR(1,100);
			MotorL(0,40);
		}else{
			MotorR(1,100);
			MotorL(1,100);
		}
	}
}

int abs(int x){
	if(x<0){
		return x*-1;
	}
	return x;
}

void getmeout(){
	int disti,distd,cont=0,temp;
	int CERCA=350;	
	int MUYCERCA=450;
	int MUYLEJOS=10;

	int CHOQUE=200;
	int NAPROX=10;

	int REFRESH=50;
	int codd=0,codi=0,odisti,odistd;

	//Hacia delante hasta encontrar algo
	while(a2d(EYE_R)<CERCA && a2d(EYE_L)<CERCA){
		MotorR(1,100);
		MotorL(1,100);
	}

	//Gira hasta colocarse ben
	while(a2d(EYE_R) < (a2d(EYE_L)+70)){
		MotorR(0,50);
		MotorL(1,50);		
	}


	while(1){
		cont++;
		disti+=a2d(EYE_L);
		distd+=a2d(EYE_R);
		
		//Hacemos la media de cada REFRESH muestras y lo usamos como medición
		if( (cont%REFRESH) == 0){

			//Almacenamos los valores antiguos
			odisti=disti; 
			odistd=distd;
			
			disti = (int)(disti / REFRESH);
			distd = (int)(distd / REFRESH);

			if(  (disti>100 && distd>100) && (disti<CHOQUE || distd<CHOQUE) && (odisti>disti || odistd>distd) /*&& (abs(disti-distd)<100)*/ ){
				codd++;
			}
			//Si el sensor izq está a una distancia de choque y la distancia actual es menor que la anterior...
			/*if(odisti<CHOQUE && odisti>disti){
				codi++;
			}else{
				codi=0;
			}

			//Si el sensor derecho está a una distancia de choque y la distancia actual es menor que la anterior...
			if(odistd<CHOQUE && odistd>distd){
				codd++;
			}else{
				codd=0;
			}*/

			putstring("Right eye:"); putnum(distd); putchar(';');
			putstring(" Left eye:"); putnum(disti);putchar(';');
			putchar('\r');

			
			//Estamos inclinados mirando la izquierda y el sensor izquierdo está muy cerca, giramos a la derecha
			if( (disti-distd)>100  && disti > MUYCERCA){
				MotorR(0,40);
				MotorL(1,40);
			}else if( (disti-distd)>100  && distd > MUYCERCA){ //Estamos inclinados mirando la derecha y el sensor derecha está muy cerca, giramos a la izq
				MotorR(1,40);
				MotorL(0,40);
			}else if(codd>NAPROX || codi>NAPROX){ /* Si nos hemos estado aproximando durante NAPROX turnos, giramos a la derecha */	
				//for(temp=0;temp<3;temp++){
					MotorR(0,40);
					MotorL(1,40);
					Delay_ms(200);
				//}
				codd=0;codi=0;
			}else{ /* Sino seguimos hacia delante girando levemente a la izquierda */
				MotorR(1,70);
				MotorL(1,40);				
			}

		}	

	}


}

void goautse(void)
{
int cont=0;
int distancia_minima=450;
int obstaculo=0;


    //Miramos inicialmente
	putstring("Right eye:"); putnum(eyeR); putchar(';');
	putstring(" Left eye:"); putnum(eyeL);putchar(';');
	putchar('\r');

	
    //Esto va hacia delante hasta que encuentre un obstaculo

   while  ( (a2d(EYE_R)<=distancia_minima) && (a2d(EYE_L)<=distancia_minima))
   {  
	MotorR(1,100);
	MotorL(1,100);
   }
 

    //Giramos a la derecha 45º la primera vez para seguir la pared
   
	MotorR(1,40);
	MotorL(0,40);

    Delay_seg(2);



//Entramos en el bucle principal, miramos izquierda y andamos...
while(1){
   	
    
	 MotorR(0,10);
	 MotorL(1,10);
	 Delay_seg(1);

	if ((a2d(EYE_R)<=distancia_minima) && (a2d(EYE_L)>=distancia_minima))
     {
     //Estamos cerca de la pared por la parte izquierda: giramos a la derecha 30º

	MotorR(1,10);
	MotorL(0,10);

    Delay_seg(1);
    

	    if ((a2d(EYE_R)>=distancia_minima) || (a2d(EYE_L)>=distancia_minima))
	     {
	     //Hay un obstaculo: giramos a la derecha 180º
	
		MotorR(1,80);
		MotorL(0,80);
	
	    Delay_seg(2); //Con el doble de tiempo giramos los 180º
	    

	     }


     }

	//Avanzamos un rato


   while  ( (a2d(EYE_R)<=distancia_minima) && (a2d(EYE_L)<=distancia_minima) && cont <=10)
   {  
	MotorR(1,100);
	MotorL(1,100);
	cont++;
	Delay_ms(100);
   }

	//Giramos a la derecha
	MotorR(0,40);
	MotorL(1,40);
    Delay_ms(1000);

	//Si no hay obstáculo
    if ((a2d(EYE_R)<=distancia_minima) && (a2d(EYE_L)<=distancia_minima))
    { 
		//Avanzamos un rato
		int cont=0;
	   while  ( (a2d(EYE_R)<=distancia_minima) && (a2d(EYE_L)<=distancia_minima) && cont <=10)
	   {  
		MotorR(1,100);
		MotorL(1,100);
		cont++;
		Delay_ms(100);
   }		
	
	}
         

}
  	
   
}

//****************************************************************************
//	Funcion principal
//****************************************************************************
void main(void)
{
	PORTA=0x00;
	PORTB=0x00;
	PORTC=0x00;
	PORTD=0x00;
	PORTE=0x00;
	ADCON1=0x80;
	TRISA=0x3F;
	TRISB=0xE1;
	TRISC=0x80;
	TRISD=0x00;
	TRISE=0x07;

	TMR1IE=1;			//Activo interrupcion del TIMER_1
	OPTION=0x00;		//Prescaler 1:1 asignado al Timer0

	/*Configuramos puerto serie*/
	SPBRG=0x20;			//Baudios:38400
	TXSTA=0b00100100;
	RCSTA=0b10010000;
	cont=0;
	segundo=0;
	MotorR(0,0);		// Inicialmente MotorR <motor derecho> (dir <0 avance,1 retroceso>, vel <entre 0 y 100>)
	MotorL(0,0);		// Inicialmente MotorL <motor izquierdo> (dir <0 avance,1 retroceso>, vel <entre 0 y 100>)
	//Incremento variable al pulsar Reset
	Reset=read_EE(0xFF);
	Reset++;
	write_EE(0xFF, Reset);
	
	T1CON=0x31;			//Prescaler de Timer1 a (Fosc/4)/8, Activo Timer1
	PEIE=1;				//Activo interrupcion externa
	GIE=1;				//Activo interrupciones

    lineR = a2d(LINE_R);//Lectura sensor optico derecho <0 luz, 255 oscuridad>
    lineC = a2d(LINE_C);//Lectura sensor optico central <0 luz, 255 oscuridad>
    lineL = a2d(LINE_L);//Lectura sensor optico izquierdo <0 luz, 255 oscuridad>
    lineAvg=lineR;
    lineAvg+=lineC;
    lineAvg+=lineL;
    lineAvg/=3;
    media=(unsigned char)lineAvg;
	
	putstring(Mensaje);
	//MAIN
	while(1)
	{
		switch(Reset)
		{
		//	case 1:
		//followTheLeader();
      //    goautse();      
				//test_motores();
		//		break;
        //  	case 3:
	//			test_infrarrojos();
		//		break;
			case 1:
				//getmeout();
				//testeyes();
				//goautse();
			poltergeist();
/*	for(i=0;i<=10;i++){
				MotorR(1,100);
	            MotorL(1,100);
                Delay_ms(100);
}
     for(i=0;i<=3;i++){
				MotorR(1,100);
	            MotorL(0,100);
                Delay_ms(100);
}	
	for(i=0;i<=10;i++){			
				MotorR(0,100);
	            MotorL(0,100);
                Delay_ms(100);
		}		*/
				break;
			case 2:
			test_infrarrojos();
			break;
		/*	case 4:
			case 6:
				//Pause
				//MotorR(0,0);
				//MotorL(0,0);
				break;*/
			default:
				//MotorR(0,0);
				//MotorL(0,0);
				InitReset();
		}
	}
}