//////////////////////////////////////////////////////////////////////////
// Programa para el AWD azul.
// www.jmnlab.com 
// Marzo 2009
// Control proporcional derivativo. 16 MHz. 
//////////////////////////////////////////////////////////////////////////

#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);
void obtener_errord(void);
void obtener_posicion(void);
void obtener_velocidad(void);
void determinar_estado(void);
void reseteo (void);
void leerswitches (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 	 // Rojo
#define LED2 PORTEbits.RE1	 // Verde
#define MINIZ PORTBbits.RB4	 // Control miniz


#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 SW1 PORTAbits.RA0 //1 Activos a nivel bajo.
#define SW2 PORTAbits.RA1 //2
#define SW3 PORTAbits.RA3 //3


//Variables
unsigned char DelayCounter1;
int servo = 110, velocidad = 110;
int errors=0;
int kp=1;
int kd=1;
int errors_pasado=0;
int errord=0;
int tic=0;
char estado=2; 
char tac=0;
char tic_freno=50;
int tac_velocidad=0;
//char freno=0;
char ultimosensor=3;
char contador=0;
char centro=0;
char nop=1;
char bloqueo=0;
int errord_pasado=0;
char interruptor=0;
int velocidad1=0;
int velocidad1l=0;
int velocidad2=0;
int velocidad2l=0;
int velocidad3=0;
int velocidad4=0;
int velocidad5=0;
int velocidad6=0;
int velocidad7=0;
int kp1=0;
int kp2=0;
int kp3=0;
int kp4=0;
int kp5=0;
int kp6=0;
int kp7=0;
char programa=0;
char seleccion=0;
int tacelera=0;
char tcambio=0;
//char giromaximo=0;


//Programa principal

void main (void)
    {
        inicializar();
//		reseteo();		
		estado=2;	
		tic=9;
		leerswitches();



        while(1) //Main Loop.
        {
			if((R0==1)||(L0==1)) //Para cambiar estados.
				centro=1;

			if((R5==1)||(R4==1)||(R3==1))
				ultimosensor=1;
		    if((L5==1)||(L4==1)||(L3==1))
				ultimosensor=0;
        }
    }

//******************************FUNCIONES*******************************************

//***************************** Hardware micro *************************************
void inicializar (void)
{
    TRISA = 0b11111111;      // Configuración de los puertos
    TRISB = 0b11101111;      //
    TRISC = 0b11111111;      //
    TRISD = 0b11111111;      //
    TRISE = 0b00000100;      // Puerto E como salidas.
    ADCON1= 0b00000111;     // Todo puerto A como digitales.//00001111
    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;
}

//**************************** Señal de mando******************************************

#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();
   obtener_errord();
   determinar_estado();
   obtener_velocidad();
   obtener_posicion(); 

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

//*********************************** Proporcional*********************************

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((R0==1)&&(contador<4))
            {
            errors+=(-1);
            contador= contador+1;
            }
        if((L0==1)&&(contador<4))
            {
            errors+=1;
            contador= contador+1;
            }
        if((R1==1)&&(contador<4))
            {
            errors+=(-3);
            contador= contador+1;
            }
        if((L1==1)&&(contador<4))
            {
            errors+=3;
            contador= contador+1;
            }
        if((R2==1)&&(contador<4))
            {
            errors+=(-5);
            contador= contador+1;
            }
        if((L2==1)&&(contador<4))
            {
            errors+=5;
            contador= contador+1;
            }
        if((R3==1)&&(contador<4))
            {
            errors+=(-7);
            contador= contador+1;
            }
        if((L3==1)&&(contador<4))
            {
            errors+=7;
            contador= contador+1;
            }
        if((R4==1)&&(contador<4))
            {
            errors+=(-9);
            contador= contador+1;
            }
        if((L4==1)&&(contador<4))
            {
            errors+=9;
            contador= contador+1;
            }
        if((R5==1)&&(contador<4))
            {
            errors+=(-11);
            contador= contador+1;
            }
        if((L5==1)&&(contador<4))
            {
            errors+=11;
            contador= contador+1;
            }
		
		if(contador!=0)
			errors=errors/contador;
	
		if(contador==0)
		{
			if(ultimosensor==0) //Invertidos, sólo cuentan para la derivada en errors_pasado.
				errors=-13;  
			else
				errors=13;  
		}
}

		
//****************************** Derivada **************************************

void obtener_errord(void) //Comprobar si ha cambiado el error y su derivada.V: errors
{

		if (errors_pasado==errors)
			{
				tic--;
				if (tic<0)
					tic=0;
				errord=0;
			}
		else if(errors_pasado != errors)
			{
				errord=(errors_pasado-errors)*tic;
				tic=9;  //9
			}

		errors_pasado=errors;
}

//************************************ Regulador *********************************

void obtener_posicion(void) //Control proporcional derivativo. Variables errors, errord.
{
		if(bloqueo==0)
		{
        if (contador!=0)
            {
			servo = 103 - errors*kp + errord*kd; //servo=centro-error*constante proporcional + derivada error* constante derivativa

            if (servo>160)
                servo=160;
            if (servo<60)
                servo=60;
            }

	       if (contador==0)
            {
			if ((ultimosensor==1)&&(estado==5))
					servo=160;
			else if((ultimosensor==0)&&(estado==5))
					servo=60;
            }

/*        if ((contador==0)&&(giromaximo==0))
            {
			if ((ultimosensor==1)&&(estado==5))
					servo=160;
			else if((ultimosensor==0)&&(estado==5))
					servo=60;
            }


 /*       if ((contador==0)&&(giromaximo==1))
            {
			if ((ultimosensor==1)&&((estado==3)||(estado==4)||(estado==5)))
					servo=160;
			else if((ultimosensor==0)&&((estado==3)||(estado==4)||(estado==5)))
					servo=60;
            }*/
            
		}
}

//*********************************** Ajuste de velocidad *******************************

void obtener_velocidad(void) //Ajuste de velocidad, Freno, Carril.
{
		kd=1;
	if(estado==1)
		{
//			tic_freno=100;
			velocidad=velocidad1;

			tac_velocidad++;

            if(tac_velocidad>200)
				tac_velocidad=200;

			if(tac_velocidad>tacelera)
				velocidad=velocidad1l;

//			if((errors<=6)&&(errors>=(-6)))
//				kp=kp1;
//			else
				kp=kp1;
		}

	else if(estado==2)
		{
//			tic_freno=100;
			velocidad=velocidad2;

			tac_velocidad++;

            if(tac_velocidad>200)
				tac_velocidad=200;

			if(tac_velocidad>tacelera)
				velocidad=velocidad2l;

//			if((errors<=4)&&(errors>=(-4)))
//				kp=1;
//			else
				kp=kp2;
		}

	else if(estado==3)
		{
//			tic_freno=100;
			tac_velocidad=0;		//probar quitando y sin quitar********************
			velocidad=velocidad3;
			kp=kp3;
		}

	else if(estado==4)
		{
//			tic_freno=100;
			tac_velocidad=0; 		//probar quitando y sin quitar********************
			velocidad=velocidad4;
			kp=kp4;
		}

	else if(estado==5)
		{
/*			if(errord_pasado>100)
				tic_freno=0;
			if(tic_freno<100)
				{
					tic_freno++;
					velocidad=151;
				}
			else*/
			tac_velocidad=0;
				velocidad=velocidad5;
			kp=kp5;

		}

	else if(estado==6)
		{
//			tic_freno=100;
//			tac_velocidad=0; //no comentado en los videos
			velocidad=velocidad6;
		}

	else if(estado==7)
		{
//			tic_freno=100;
//			tac_velocidad=0;
			velocidad=velocidad7;
			kp=kp7;
		}
	errord_pasado=errord;
}

//********************************** Determinar estado ***********************************

//1 exterior, 2 interior, 3 interior cambio, 4 exterior cambio, 5 freno.

void determinar_estado(void)
{
//estado inicial interior, sentido: parte derecha en el interior coche avanza. Variables de entrada velocidad, errors y errorp.
//estado anterior.

switch (estado)
{
	case 1: //carril exterior
	{
	    LED1=1;
		nop=2;	
		centro=0;
		LED2=0;
		
		tac++;
		if(tac>tcambio)  //*******************tiempo de cambio
		{
			tac=0;
			if(programa==1)
				estado=6;
		}

		if(contador==0)
		{
			if(ultimosensor==1)
				estado=4;
			else if(ultimosensor==0)
				estado=5;
		}	
	}break;

	case 2: //carril interior
	{
	    LED1=0;
		nop=2;	
		centro=0;
		tac=0;
		LED2=1;


		if(contador==0)
		{
			if(ultimosensor==1)
				estado=5;
			else if(ultimosensor==0)
				estado=3;
		}
	}break;

	case 3: //carril interior cambia carril exterior o sigue en el interior
	{
	    LED1=1;
		nop=2;	
		tac=0;
		LED2=1;

		if(centro==1)
		{
			if(ultimosensor==1)
				estado=1; //carril exterior
			else if(ultimosensor==0)
				estado=2; //carril interior
		}
		if(errors<0)
			errors=errors*(-1);
	}break;

	case 4: //carril exterior cambia a carril interior o sigue en el exterior
	{
	    LED1=1;
		nop=2;	
		tac=0;
		LED2=1;

		if(centro==1) //Ponerlo en el main() para evitar saltarlo en 16 ms
		{
			if(ultimosensor==1)  //exterior
				estado=1;
			else if(ultimosensor==0) //interior
				estado=2;
		}
		if(errors>0)
			errors=errors*(-1);
	}break;

	case 5: //freno
	{
	    LED1=0;
		nop=2;	
		tac=0;
		LED2=0;

		//if(contador!=0)
		if(centro==1)
		{
			if(ultimosensor==1)
				estado=2; 	//interior		
			else if(ultimosensor==0)
				estado=1;	//exterior
		}
	}break;

	case 6: //cambio de exterior a interior forzado
	{
		LED1=0;
		bloqueo=1;
		servo=96;
		LED2=0;
		if(contador==0)
		{
			bloqueo=0;
			centro=0;
			if(ultimosensor==0)
				estado=5;
			else if(ultimosensor==1)
				estado=7;
		}
	}break;

	case 7: //carril exterior cambia a carril interior forzado
			//como el caso 3, lo dejo por los leds, frenar y valor de la velocidad.
	{
	    LED1=1;
		nop=2;	
		tac=0;
		LED2=1;

		if(centro==1) //Ponerlo en el main() para evitar saltarlo en 16 ms
		{
			if(ultimosensor==1)  //exterior
				estado=1;
			else if(ultimosensor==0) //interior
				estado=2;
		}
		if(errors<0)
			errors=errors*(-1);
	}break;



	default: break;
}
}

//******************Leer Interruptores********************
void leerswitches (void)
{
		//Leer microinterruptores.
		seleccion=0;

		if(SW2==0)
			seleccion= seleccion + 1;
		if(SW3==0)
			seleccion= seleccion +2;

		switch(seleccion) //1. Exterior. 2. Interior. 3. Interior->. 4. Exterior->. 5. Freno. 6. Cambio F. 7.Exterior->F.
		{
		case 0: //normal.
		{
		velocidad1=99;  //Exterior inicio	
		velocidad1l=101;//Exterior
		velocidad2=99;	//Interior inicio
		velocidad2l=101;//Interior
		velocidad3=101;	//Interior a exterior
		velocidad4=100;	//Exterior a interior
		velocidad5=103;	//Freno	
		velocidad6=101;	//Cambio exterior
		velocidad7=101;	//Cambio centro

		kp1=1;
		kp2=2;  //más giro en el interior.
		kp3=1;
		kp4=1;
		kp5=3;
		kp6=1;
		kp7=2;

		tcambio=50;
		tacelera=20;
		

		}break;

		case 1: //más rápido.
		{
		velocidad1=98;
		velocidad1l=100;
		velocidad2=98;
		velocidad2l=100; 
		velocidad3=100; //
		velocidad4=99; //
		velocidad5=102; //
		velocidad6=100;
		velocidad7=100;

		kp1=1;
		kp2=2;
		kp3=1;
		kp4=1;
		kp5=3;
		kp6=1;
		kp7=2;

		tcambio=50;
		tacelera=20;

		}break;

		case 2: //más rápido.
		{
		velocidad1=98;
		velocidad1l=100;
		velocidad2=98;
		velocidad2l=101; 
		velocidad3=100; //
		velocidad4=99; //
		velocidad5=102; //
		velocidad6=100;
		velocidad7=100;

		kp1=1;
		kp2=2;
		kp3=1;
		kp4=1;
		kp5=3;
		kp6=1;
		kp7=2;

		tcambio=50;
		tacelera=20;

		}break;

		case 3: //más rápido.
		{
		velocidad1=98;
		velocidad1l=99;
		velocidad2=98;
		velocidad2l=100;
		velocidad3=99;
		velocidad4=99;
		velocidad5=101;
		velocidad6=99;
		velocidad7=99;

		kp1=1;
		kp2=2;
		kp3=1;
		kp4=1;
		kp5=3;
		kp6=1;
		kp7=2;

		tcambio=40;
		tacelera=20;

		}break;

/*		case 3: //Homologación
		{
		velocidad1=102;
		velocidad1l=102;
		velocidad2=102;
		velocidad2l=102;
		velocidad3=102;
		velocidad4=102;
		velocidad5=102;
		velocidad6=102;
		velocidad7=102;

		kp1=1;
		kp2=1;
		kp3=1;
		kp4=1;
		kp5=3;
		kp6=1;
		kp7=1;

		tcambio=70;
		tacelera=20;

		}break; 
*/


		default:break;
		}

		if(SW1==1)
		{
			programa=1;  //programa=1 se cambia de carril
			estado=2;	//interior
		}

		if(SW1==0)
		{
			programa=0;  //programa=0 no se cambia de carril
			estado=1;	//exterior
			kp2=1;
		}

}

//******************** Reset *******************************

void reseteo (void)
{

    INTCONbits.GIE = 0;
    LED2 = 1;                  //3 parpadeos para detectar los posibles resets del pic
    LED1 = 0;
    Delay10KTCYx(200);
    LED2 = 0;
    Delay10KTCYx(200);
    LED2 = 1;
    Delay10KTCYx(200);
    LED2 = 0;
    Delay10KTCYx(200);
    LED2 = 1;
    Delay10KTCYx(200);
    LED2 = 0;
    INTCONbits.GIE = 1;
}