#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

//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 SnsCentDcho = PORTA.1

//Colores
#define Fondo 0
#define Pista 1

//Configura el microcontrolador
inline void Config() {
	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;
}

//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;
}

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


void main(){
	Config();

	while(1){
		if(SnsCentDcho==Pista) {
			MotIzqAvanzar();
			MotDerAvanzar();
		} else {
			MotIzqDetener();
			MotDerDetener();
		}
	}
}