/*******************************************************************************
 *
 *              ROBOT QUE EVITA OBSTACULOS
 *
 *******************************************************************************
 * FileName:        mcu.c
 * Processor:       PIC18F1320
 * Complier:        XC8 v1.32
 * Author:          Pedro Sánchez (MrChunckuee)
 * Blog:            http://mrchunckuee.blogspot.com/
 * Email:           mrchunckuee.psr@gmail.com
 * Description:     Robot que evita obstaculos a una distancia de 15cm
 *                  utiliza un sensor ultrasonico HC-SR04
 *                  OSC Interno = 8MHz
 ******************************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <xc.h>
#include "mcu.h"

#define _XTAL_FREQ 8000000

enum{AVANZAR, RETROCEDER, GIRO_DER, GIRO_IZQ, PARO};

unsigned int DISTANCIA, DISTANCIA_DER, DISTANCIA_IZQ;

void MCU_Init(void){
    OSCCONbits.IRCF=0b111;  //Oscilador interno a 8MHz
    ADCON1=0xFF; //Configuramos entrada/salida digitales

    //Definimos entradas y salidas
    TRIS_MOTOR_IZQUIERDA_INA=OUTPUT;
    TRIS_MOTOR_IZQUIERDA_INB=OUTPUT;
    TRIS_MOTOR_DERECHA_INA=OUTPUT;
    TRIS_MOTOR_DERECHA_INB=OUTPUT;
    TRIS_LED_DERECHA=OUTPUT;
    TRIS_LED_IZQUIERDA=OUTPUT;
    TRIS_TRIG=OUTPUT; //Pin TRIG del HC-SR04 como salida
    TRIS_ECHO=INPUT; //Pin ECHO del HC-SR04 como entrada

    //Limpiamos los puertos
    PORTA=CLEAR;
    PORTB=CLEAR;
}

void Navegacion(void){
    DISTANCIA=Medir_Distancia();//Mido la distancia
    if(DISTANCIA>15){ //Si la distancias es mayor a 15cm
        Opcion_Movimiento(AVANZAR);//Robot avanza
    }
    else {//Si es menor a 15cm
        Opcion_Movimiento(PARO);//Robot se detiene
        Opcion_Movimiento(GIRO_DER);//Gira hacia la derecha
        Delay_MS(1000);//Retardo de 1 segundo mientras gira a la derecha
        Opcion_Movimiento(PARO);//Se detiene
        DISTANCIA_DER=Medir_Distancia();//Y mide la distancia 
        Opcion_Movimiento(GIRO_IZQ);//Gira a la izquierda
        Delay_MS(2000);//Retardo de 2 segundos mientras gira a la izquierda
        Opcion_Movimiento(PARO);//Se detiene
        DISTANCIA_IZQ=Medir_Distancia();//Mide la distancia una vez mas
        Opcion_Movimiento(GIRO_DER);//Gira a la derecha para regresar
        Delay_MS(1000);//Retardo de 1 segundo mientras vuelve al origen
        Opcion_Movimiento(PARO);//Se detiene
        //Compara las distancias medidas
        //Si distancia derecha es mayor a distancia izquierda
        if(DISTANCIA_DER>=DISTANCIA_IZQ){
            Opcion_Movimiento(GIRO_DER);//Gira a la derecha
            Delay_MS(1000);//Retardo de 1 segundo mientras gira
            Opcion_Movimiento(PARO);//Se para
        }
        else{//Si ditancia derecha es menor a distancia izquierda
            Opcion_Movimiento(GIRO_IZQ);//Gira a la izquierda
            Delay_MS(1000);//Retardo de 1 segundo mientras gira
            Opcion_Movimiento(PARO);//Se para
        }
        //Termina esto y vuelve a repetir las intrucciones
    }
}

//Rutina para retardos en mS personalizada
void Delay_MS(unsigned int t){
    unsigned int j;
    for(j=0;j<t;j++){
        __delay_ms(1);
    }
}

unsigned int Medir_Distancia(void){
    unsigned int CENTIMETROS=0;
    TRIG=HIGH;
    __delay_us(10); //Retardo de 10uS
    TRIG=LOW;
    while(ECHO==LOW); //Espera flanco de subida por el pin echo
    while(ECHO==HIGH){
        CENTIMETROS++;
        __delay_us(58);
    }
return(CENTIMETROS);
}

void Opcion_Movimiento(unsigned char X){
    switch(X){
        case AVANZAR:
            MOTOR_IZQUIERDA_INA=LOW;
            MOTOR_IZQUIERDA_INB=HIGH;
            MOTOR_DERECHA_INA=LOW;
            MOTOR_DERECHA_INB=HIGH;
        break;
        case RETROCEDER:
            MOTOR_IZQUIERDA_INA=HIGH;
            MOTOR_IZQUIERDA_INB=LOW;
            MOTOR_DERECHA_INA=HIGH;
            MOTOR_DERECHA_INB=LOW;
        break;
        case GIRO_DER:
            MOTOR_IZQUIERDA_INA=LOW;
            MOTOR_IZQUIERDA_INB=HIGH;
            MOTOR_DERECHA_INA=HIGH;
            MOTOR_DERECHA_INB=LOW;
        break;
        case GIRO_IZQ:
            MOTOR_IZQUIERDA_INA=HIGH;
            MOTOR_IZQUIERDA_INB=LOW;
            MOTOR_DERECHA_INA=LOW;
            MOTOR_DERECHA_INB=HIGH;
        break;
        case PARO:
            MOTOR_IZQUIERDA_INA=LOW;
            MOTOR_IZQUIERDA_INB=LOW;
            MOTOR_DERECHA_INA=LOW;
            MOTOR_DERECHA_INB=LOW;
        break;
        default:
        break;
    }
}