#include "16f722.h"
#fuses NOWDT,PUT,NOBROWNOUT,NOPROTECT,INTRC_IO,PLLEN,MCLR,NOVCAP
#use delay(clock=16000000)

#byte PORTA = 0x05
#byte PORTB = 0x06
#byte PORTC = 0x07
#byte TRISA = 0x85
#byte TRISB = 0x86
#byte TRISC = 0x87
#byte OSCCON = 0x90
#byte ANSELA = 0x185

//Motores
#bit MotIzqEnable = PORTC.1
#bit MotIzqIn1 = PORTC.0
#bit MotIzqIn2 = PORTC.3
#bit MotDerEnable = PORTC.2
#bit MotDerIn1 = PORTC.5
#bit MotDerIn2 = PORTC.4

//Sensores
#bit SnsLatIzq = PORTA.5 //sensor lateral izquierdo
#bit SnsIntrmIzq = PORTA.4 //sensor intermedio izquierdo
#bit SnsCentIzq = PORTA.2 //sensor central izquierdo
#bit SnsCentDcho = PORTA.1
#bit SnsIntrmDcho = PORTA.3
#bit SnsLatDcho = PORTA.0

//Colores
#define Fondo 0
#define Pista 1

//Configura el microcontrolador
inline void Config() {
	ANSELA=0; //desactiva entradas analogicas
	TRISA = 0xFF; //todos los bits del PORTA como entradas
	TRISC = 0b11000000;
	OSCCON = 0b00110000; //oscilador interno a 16MHz
}

//Motor izquierdo: Avanzar
void MotIzqAvanzar() {
	MotIzqEnable=1;
	MotIzqIn1=1;
	MotIzqIn2=0;
}

//Motor izquierdo: Retroceder
void MotIzqRetroceder() {
	MotIzqEnable=1;
	MotIzqIn1=0;
	MotIzqIn2=1;
}

//Bloquea el motor izquierdo (detencion rapida)
void MotIzqDetener() {
	MotIzqEnable=1;
	MotIzqIn1=0;
	MotIzqIn2=0;
}

//Motor derecho: Avanzar
void MotDerAvanzar() {
	MotDerEnable=1;
	MotDerIn1=1;
	MotDerIn2=0;
}

//Motor derecho: Retroceder
void MotDerRetroceder() {
	MotDerEnable=1;
	MotDerIn1=0;
	MotDerIn2=1;
}

//Bloquea el motor derecho (detencion rapida)
void MotDerDetener() {
	MotDerEnable=1;
	MotDerIn1=0;
	MotDerIn2=0;
}

//Hace que el robot se mueva en direccion recta hacia delante
void Avanzar(){
	MotDerAvanzar();
	MotIzqAvanzar();
}

//Girar hacia la derecha
void GirarDcha(){
	MotDerDetener();
	MotIzqAvanzar();
}

//Girar hacia la izquierda
void GirarIzq(){
	MotIzqDetener();
	MotDerAvanzar();
}

//Girar hacia la derecha sobre si mismo
void RotarDcha(){
	MotDerRetroceder();
	MotIzqAvanzar();
}

//Girar hacia la izquierda sobre si mismo
void RotarIzq(){
	MotIzqRetroceder();
	MotDerAvanzar();
}

//Se detiene completamente
void Detener(){
	MotIzqDetener();
	MotDerDetener();
}

void main(){
	Config();

	while(1){
		if(SnsCentIzq==Pista && SnsCentDcho==Pista) Avanzar();
		else if(SnsCentIzq==Fondo && SnsCentDcho==Fondo) Detener();
		else if(SnsIntrmIzq==Fondo) RotarDcha();
		else if(SnsIntrmDcho==Fondo) RotarIzq();
		else if(SnsCentIzq==Fondo) GirarDcha();
		else if(SnsCentDcho==Fondo) GirarIzq();
	}
}