// CONFIG1H

#pragma config OSC = HSPLL // Oscillator Selection bits (HS oscillator with
PLL enabled/Clock frequency = (4 x FOSC))
#pragma config OSCS = OFF // Oscillator System Clock Switch Enable bit
(Oscillator system clock switch option is disabled (main oscillator is source))

// CONFIG2L
#pragma config PWRT = OFF
#pragma config BOR = ON
enabled)
#pragma config BORV = 20
// Power-up Timer Enable bit (PWRT disabled)
// Brown-out Reset Enable bit (Brown-out Reset
// Brown-out Reset Voltage bits (VBOR set to 2.0V)

// CONFIG2H
#pragma config WDT = OFF // Watchdog Timer Enable bit (WDT disabled (control
is placed on the SWDTEN bit))
#pragma config WDTPS = 128 // Watchdog Timer Postscale Select bits (1:128)

// CONFIG3H
#pragma config CCP2MUX =ON // CCP2 Mux bit (CCP2 input/output is multiplexed
with RB3)

// CONFIG4L
#pragma config STVR = OFF // Stack Full/Underflow Reset Enable bit (Stack
Full/Underflow will not cause RESET)
#pragma config LVP = OFF // Low Voltage ICSP Enable bit (Low Voltage ICSP
disabled)

// CONFIG5L
#pragma config CP0 = OFF // Code Protection bit (Block 0 (000200-001FFFh)
not code protected)
#pragma config CP1 = OFF // Code Protection bit (Block 1 (002000-003FFFh)
not code protected)
#pragma config CP2 = OFF // Code Protection bit (Block 2 (004000-005FFFh)
not code protected)
#pragma config CP3 = OFF // Code Protection bit (Block 3 (006000-007FFFh)
not code protected)

// CONFIG5H
#pragma config CPB = OFF // Boot Block Code Protection bit (Boot Block
(000000-0001FFh) not code protected)
#pragma config CPD = OFF // Data EEPROM Code Protection bit (Data EEPROM not
code protected)

// CONFIG6L
#pragma config WRT0 = OFF // Write Protection bit (Block 0 (000200-001FFFh)
not write protected)
#pragma config WRT1 = OFF // Write Protection bit (Block 1 (002000-003FFFh)
not write protected)
#pragma config WRT2 = OFF // Write Protection bit (Block 2 (004000-005FFFh)
not write protected)
#pragma config WRT3 = OFF // Write Protection bit (Block 3 (006000-007FFFh)
not write protected)

// CONFIG6H
#pragma config WRTC = OFF // Configuration Register Write Protection bit
(Configuration registers (300000-3000FFh) not write protected)
#pragma config WRTB = OFF // Boot Block Write Protection bit (Boot Block
(000000-0001FFh) not write protected)
#pragma config WRTD = OFF // Data EEPROM Write Protection bit (Data EEPROM
not write protected)

// CONFIG7L
#pragma config EBTR0 = OFF // Table Read Protection bit (Block 0 (000200-
001FFFh) not protected from Table Reads executed in other blocks)
#pragma config EBTR1 = OFF // Table Read Protection bit (Block 1 (002000-
003FFFh) not protected from Table Reads executed in other blocks)
#pragma config EBTR2 = OFF // Table Read Protection bit (Block 2 (004000-
005FFFh) not protected from Table Reads executed in other blocks)
#pragma config EBTR3 = OFF // Table Read Protection bit (Block 3 (006000-
007FFFh) not protected from Table Reads executed in other blocks)

// CONFIG7H
#pragma config EBTRB = OFF // Boot Block Table Read Protection bit (Boot Block
(000000-0001FFh) not protected from Table Reads executed in other blocks)

// #pragma config statements should precede project file includes.

// Use project enums instead of #define for ON and OFF.

#include <xc.h>
#define _XTAL_FREQ 40000000

//Funciones
void CONF_PORTS();
void CONF_ADC();
void CONF_USART();
void CONF_PWM();
void SENSORES();
void CLEAR();
void SEND (unsigned char dato);
int ESTABLECER_VEL_INI(void);
void ROMPER_INERCIA();
void LINEA_RECTA(unsigned char s);
void GIRAR_DER (unsigned char r);
void GIRAR_IZQ (unsigned char l);
void GIRAR_180 (unsigned char c);
void CAMBIAR_SENTIDO(void);
void CAMBIAR_SENTIDO2(void);
void FIN(void);

//Variables
unsigned char sensor = 0;
unsigned char tmp = 0;
unsigned char VAL_L = 0;
unsigned char VAL_R = 0;
unsigned char AA[10];

void main(void) {
CONF_PORTS();
CLEAR();
CONF_ADC();
CONF_USART();
CONF_PWM();
 int flag = 10; //Numero Maximo de errores

SEND('s');

do{
SEND('P');
flag = ESTABLECER_VEL_INI();
}while(flag != 0);

VAL_L = ((AA[1]-48)*100) + ((AA[2]-48)*10)+(AA[3]-48);

VAL_R = ((AA[6]-48)*100) + ((AA[7]-48)*10)+(AA[8]-48);

ROMPER_INERCIA();

while(1){

SENSORES();
if(sensor == 0b11111100 || sensor == 0b11111111 || sensor == 0b11110000 ||
sensor == 0b11111000 || sensor == 0b11111100 ){

GIRAR_DER(sensor);

}
else if(sensor == 0b00000111 || sensor == 0b00001111){
GIRAR_IZQ(sensor);
}
else if (sensor==0b00000000 ){

GIRAR_180(sensor);
}

else{

(LINEA_RECTA(sensor) );
}

}
return;
}

void CLEAR(){
ADRESH = 0;
ADRESL = 0;
LATD = 0;
PR2 = 0;
CCPR1L = 0; //PWM1
CCPR2L = 0; //PWM2
sensor = 0;
}

void CONF_PORTS(){

//76543210
 TRISA = 0b00101111; //AN4 AN3 AN2 AN1 AN0

//76543210
TRISE = 0b00000111; //AN5 AN6 AN7

//76543210
TRISC = 0b10000000; //RX = 7 TX = 6 I1 = 5 I2 = 4 I3 = 3 CCP1 = 2 CCP2 = 1 I4
= 0

void CONF_ADC(){
//76543210
ADCON0 = 0b01000001;
//76543210
ADCON1 = 0b01000000;
}

void SENSORES(){
sensor = 0;
for(unsigned char CH = 0; CH < 8; CH++){
ADCON0bits.CHS = CH; //Cambiar Canal
__delay_us(13);
ADCON0bits.GO = 1; //Iniciar Conversion
while(ADCON0bits.GO == 1);

if(ADRESH > 127){

sensor = sensor | (1 << CH);
tmp = '1';
}
else
{
tmp = '0';
}
//SEND(tmp);

//SEND(13);

void CONF_USART(){
//76543210
TXSTA = 0b00100010;
//76543210
RCSTA = 0b10010000;
SPBRG = 64;
}

void SEND(unsigned char dato){

while(PIR1bits.TXIF == 0) //Esperar a que finalice el envo del dato
{}
TXREG = dato;
}
void CONF_PWM(){
//76543210
CCP1CON = 0b00001100;
//76543210
CCP2CON = 0b00001100;
//76543210
T2CON = 0b00000111;

PR2 = 255; //Max ciclo util

}

void CAMBIAR_SENTIDO(){
//MOTOR DERECHO
CCPR1L = VAL_R+20; //CICLO UTIL

RC5 = 0;
RC4 = 1;

//MOTOR IZQUIERDO
CCPR2L = VAL_L+20; //CICLO UTIL

RC3= 1;
RC0 = 0;

}
void CAMBIAR_SENTIDO2(){
//MOTOR DERECHO
CCPR1L = VAL_R+20; //CICLO UTIL

RC5 = 1;
RC4 = 0;

//MOTOR IZQUIERDO
CCPR2L = VAL_L+20; //CICLO UTIL

RC3= 0;
RC0 = 1;

int ESTABLECER_VEL_INI(void)
{
unsigned char CONTAR_TRAMAS = 0;
unsigned char ERROR = 0;
unsigned char TEMP = 0;
unsigned char i = 0;

for(i=0;i<10;i++)
{
while(PIR1bits.RCIF == 0)
// Espera a que reciba SOLO UNA trama
{}
//SEND(i + 48);
TEMP = RCREG;
// Lee el dato recibido

PIR1bits.RCIF = 0;
// Baja bandera de dato leido
switch (CONTAR_TRAMAS)
{

case 0:{
if(TEMP == 'A')
{
AA[CONTAR_TRAMAS] = TEMP;
CONTAR_TRAMAS++;
}

else
{ERROR++;}

}break;

case 1:{
if(TEMP >= 48 && TEMP <= 50)
{
AA[CONTAR_TRAMAS] = TEMP;
CONTAR_TRAMAS++;
}

else
{ERROR++;}

}break;

case 2 :{
if(TEMP >= 48 && TEMP <= 57)
{
AA[CONTAR_TRAMAS] = TEMP;
CONTAR_TRAMAS++;
}

else
{ERROR++;}

}break;

case 3 :{
if(TEMP >= 48 && TEMP <= 57)
{
AA[CONTAR_TRAMAS] = TEMP;
CONTAR_TRAMAS++;
}

else
{ERROR++;}

}break;

case 4 :{
if(TEMP == 13)
{
AA[CONTAR_TRAMAS] = TEMP;
CONTAR_TRAMAS++;
}
 else
{ERROR++;}

}break;

case 5:{
if(TEMP == 'B')
{
AA[CONTAR_TRAMAS] = TEMP;
CONTAR_TRAMAS++;
}

else
{ERROR++;}

}break;

case 6:{
if(TEMP >= 48 && TEMP <= 50)
{
AA[CONTAR_TRAMAS] = TEMP;
CONTAR_TRAMAS++;
}

else
{ERROR++;}

}break;

case 7 :{
if(TEMP >= 48 && TEMP <= 57)
{
AA[CONTAR_TRAMAS] = TEMP;
CONTAR_TRAMAS++;
}

else
{ERROR++;}

}break;

case 8 :{
if(TEMP >= 48 && TEMP <= 57)
{
AA[CONTAR_TRAMAS] = TEMP;
CONTAR_TRAMAS++;
}

else
{ERROR++;}

}break;

case 9 :{
if(TEMP == 13)
{
AA[CONTAR_TRAMAS] = TEMP;
CONTAR_TRAMAS++;
}
 else
{ERROR++;}

}break;

default:{
ERROR++;
}

}
}

if (ERROR == 0 && CONTAR_TRAMAS ==10)

{
SEND('1');

if(ERROR != 0 && CONTAR_TRAMAS ==10)

{
SEND('2');

}
return ERROR ;

void ROMPER_INERCIA(){

//MOTOR DERECHO
CCPR1L = VAL_R; //CICLO UTIL

RC5 = 1;
RC4 = 0;

//MOTOR IZQUIERDO

CCPR2L = VAL_L; //CICLO UTIL

RC3= 1;
RC0 = 0;
}

void GIRAR_DER(unsigned char r){

//DETENER MOTORES
CCPR1L =0;
CCPR2L =0;
//ARRACAR
ROMPER_INERCIA();
__delay_ms(600);
SENSORES();
if (sensor == 0b11111111){
//DETENER MOTORES
 FIN();
}
//DETENER MOTORES
CCPR1L =0;
CCPR2L =0;
//GIRAR MOTOR
CAMBIAR_SENTIDO2();
__delay_ms(100);
do
SENSORES();
while (sensor != 0b00011000);
CCPR1L =0;
CCPR2L =0;
ROMPER_INERCIA();

void GIRAR_IZQ(unsigned char l){

//DETENER MOTORES
CCPR1L =0;
CCPR2L =0;
//ARRACAR
ROMPER_INERCIA();
__delay_ms(600);
//DETENER MOTORES
CCPR1L =0;
CCPR2L =0;
//GIRAR MOTOR
CAMBIAR_SENTIDO();
__delay_ms(100);
do
SENSORES();
while (sensor != 0b00011000);
CCPR1L =0;
CCPR2L =0;
ROMPER_INERCIA();

}
void GIRAR_180(unsigned char c){
//DETENER MOTORES
CCPR1L =0;
CCPR2L =0;
//ARRACAR
ROMPER_INERCIA();
__delay_ms(600);
//DETENER MOTORES
CCPR1L =0;
CCPR2L =0;
//GIRAR MOTOR
CAMBIAR_SENTIDO();
do
SENSORES();
while (sensor !=0b00011000);
CCPR1L =0;
CCPR2L =0;
ROMPER_INERCIA();
}
void FIN (void){
CCPR1L =0;
CCPR2L =0;

do
SENSORES();
while (sensor==0b11111111);

}
void LINEA_RECTA(unsigned char s){
switch(s){

// Derecha
case 0b01100000:{
//SEND('1');
CCPR2L = (VAL_L - 55);
CCPR1L = (VAL_R + 55);

}break;

case 0b00100000:{
//SEND('2');
CCPR2L = (VAL_L - 45);
CCPR1L = (VAL_R + 45);
}break;

case 0b00110000:{
//SEND('3');
CCPR2L = (VAL_L - 35);
CCPR1L = (VAL_R + 35);
}break;

case 0b00010000:{
//SEND('4');
CCPR2L = (VAL_L - 25);
CCPR1L = (VAL_R + 25);
}break;

case 0b00011000:{
//SEND('5');
CCPR2L = (VAL_L + 30); //Centro
CCPR1L = (VAL_R + 30);
}break;

// izquierda
case 0b00001000:{
//SEND('6');
CCPR2L = (VAL_L + 25);
CCPR1L = (VAL_R - 25);
}break;

case 0b00001100:{
//SEND('7');
CCPR2L = (VAL_L + 35);
CCPR1L = (VAL_R - 35);
}break;
 case 0b00000100:{
//SEND('8');
CCPR2L = (VAL_L + 45);
CCPR1L = (VAL_R - 45);
}break;

case 0b00000110:{
//SEND('9');
CCPR2L = (VAL_L + 50);
CCPR1L = (VAL_R - 50);
}break;
default:break;
}