//////////////////////////////////////////////////////////////////////////
// Programa para el AWD
// www.jmnlab.com 
// febrero 2009
// Control proporcional
//////////////////////////////////////////////////////////////////////////

#include <p18f452.h>
#include <delays.h>
#include <timers.h>

#pragma config WDT=OFF, LVP=OFF, OSC=HS, OSCS=OFF, PWRT=ON, BOR=OFF, STVR=ON

void inicializar (void);
void timer2_isr (void);
void obtener_error(void);

#pragma code high_vector=0x08 // high interrupt vector en 0008h
void interrupt (void)
{
    _asm GOTO timer2_isr _endasm // Salta a la ISR
}
#pragma code 


#define LED1 PORTEbits.RE0 // Ambar
#define LED2 PORTEbits.RE1 // Rojo
#define SENSORES PORTEbits.RE2 // Placa de sensores
#define MINIZ PORTBbits.RB4 // Control miniz

#define	   R7 PORTCbits.RC0 //Extremo derecho
#define    R6 PORTCbits.RC1
#define    R5 PORTCbits.RC2
#define    R4 PORTCbits.RC3
#define    R3 PORTDbits.RD0
#define    R2 PORTDbits.RD1
#define    R1 PORTDbits.RD2
#define    R0 PORTDbits.RD3 // Centrales
#define    L0 PORTCbits.RC4 //
#define    L1 PORTCbits.RC5
#define    L2 PORTCbits.RC6
#define    L3 PORTCbits.RC7
#define    L4 PORTDbits.RD4
#define    L5 PORTDbits.RD5
#define    L6 PORTDbits.RD6
#define    L7 PORTDbits.RD7 // Extremo izquierdo

//Variables
unsigned char DelayCounter1;
int servo = 108, velocidad = 110;
int contador=0;
int errors=0;
char ultimosensor=3;


//Programa principal

void main (void)
    {
        inicializar();
        LED1 = 1;
        SENSORES=1;
		velocidad=101;

        while(1)
        {
			if(R7==1)
				ultimosensor=1;
			else if(L7==1)
				ultimosensor=0;
        }
    }

//******************************FUNCIONES*******************************************
void inicializar (void)
{
    TRISA = 0b11111111;      // Configuración de los puertos
    TRISB = 0b11101111;      //
    TRISC = 0b11111111;      //
    TRISD = 0b11111111;      //
    TRISE = 0b00000000;      // Puerto E como salidas.
    ADCON1 = 0b00001111;     // Todo puerto A como digitales.
    PORTB = 0;
    PORTE = 0;
    OpenTimer2 (TIMER_INT_ON & T2_PS_1_16 & T2_POST_1_16); //TMR2IF=0 TMR2IE=1 TMR2ON=1 TMR2=0
    RCONbits.IPEN = 0;
    INTCONbits.GIE = 1;
    INTCONbits.PEIE = 1;
    PR2 = 250;
}

#pragma interrupt timer2_isr save=DelayCounter1  // interrupción, salvamos la variable de los delays

void timer2_isr ( void)  // 24 44 64 //60 110 160

{
   PIR1bits.TMR2IF = 0;  //Limpia el flag de interrupción del timer2
   obtener_error();

   MINIZ = 1;
   Delay100TCYx(16);      // 400 uS
   MINIZ = 0;
   Delay10TCYx(servo);
   Delay10TCYx(servo);
   Delay10TCYx(servo);
   Delay10TCYx(servo);
   MINIZ = 1;
   Delay100TCYx(16);      //400 uS
   MINIZ = 0;
   Delay10TCYx(velocidad); 
   Delay10TCYx(velocidad);
   Delay10TCYx(velocidad);
   Delay10TCYx(velocidad);  
   MINIZ = 1;
   Delay100TCYx(16);   
   MINIZ = 0;   
}

void obtener_error(void)
{
        errors=0;
        contador=0;


//Asignar error, 3 sensores consecutivos, máximo activados, valores para obtener siempre entero en la división.

		if ((L0==1)&&(R0==1))
				servo=108;
		else if((L0==0)||(R0==0))
		{

       if((L7==1)&&(contador<3))
            {
            errors+=15;
            contador++;
            }
        if((L6==1)&&(contador<3))
           {
            errors+=13;
           contador= contador+1;
            }
        if((L5==1)&&(contador<3))
            {
            errors+=11;
            contador= contador+1;
            }
        if((L4==1)&&(contador<3))
            {
            errors+=9;
            contador= contador+1;
            }
        if((L3==1)&&(contador<3))
            {
            errors+=7;
            contador= contador+1;
            }
        if((L2==1)&&(contador<3))
            {
            errors+=5;
            contador= contador+1;
            }
        if((L1==1)&&(contador<3))
            {
            errors+=3;
            contador= contador+1;
            }
        if((L0==1)&&(contador<3))
            {
            errors+=1;
            contador= contador+1;
            }
        if((R0==1)&&(contador<3))
            {
            errors+=(-1);
            contador= contador+1;
            }
        if((R1==1)&&(contador<3))
            {
            errors+=(-3);
            contador= contador+1;
            }
        if((R2==1)&&(contador<3))
            {
            errors+=(-5);
            contador= contador+1;
            }
        if((R3==1)&&(contador<3))
            {
            errors+=(-7);
            contador= contador+1;
            }

        if((R4==1)&&(contador<3))
            {
            errors+=(-9);
            contador= contador+1;
            }
        if((R5==1)&&(contador<3))
            {
            errors+=(-11);
            contador= contador+1;
            }
        if((R6==1)&&(contador<3))
            {
            errors+=(-13);
            contador= contador+1;
            }
        if((R7==1)&&(contador<3))
            {
            errors+=(-15);
            contador++;
            }

		errors=errors/contador;


//Control proporcional.

        if (contador!=0)
            {
			LED2=0;
			servo = 108 - errors*3; //servo=centro-error*constante proporcional
            if (servo>160)
                servo=160;
            if (servo<60)
                servo=60;
            }
        else if (contador==0)
            {
			LED2=1;
			if (ultimosensor==1)
					servo=160;
			else if(ultimosensor==0)
					servo=60;
            }
		}
}