#include <16C77.H>

#fuses HS, NOWDT, NOPROTECT, NOBROWNOUT
#use delay(clock=20000000)
#use rs232(baud=115200, xmit=PIN_C6, rcv=PIN_C7, BITS=8, PARITY=N)


#use fast_io(A)		// acceso al puerto sin programar TRIS_A
#use fast_io(B)		// acceso al puerto sin programar TRIS_B
#use fast_io(C)		// acceso al puerto sin programar TRIS_C
#use fast_io(D)		// acceso al puerto sin programar TRIS_D
#use fast_io(E)		// acceso al puerto sin programar TRIS_E

/**************************************************************************/
/************************ DEFINICION DE CONSTANTES ************************/
/**************************************************************************/

/************************* CONSTANTES DE LA CAMARA ************************/

#define ENABLE PIN_C5
#define RES PIN_D4
#define DES PIN_C4
#define RESOUT PIN_B1
#define OUT PIN_A0

#define COL0 PIN_B2
#define COL1 PIN_B3
#define COL2 PIN_B4
#define COL3 PIN_B5
#define COL4 PIN_A2
#define COL5 PIN_A4
#define COL6 PIN_A5

#define FIL0 PIN_E0
#define FIL1 PIN_D3
#define FIL2 PIN_D2
#define FIL3 PIN_D5
#define FIL4 PIN_D6
#define FIL5 PIN_D7
#define FIL6 PIN_B0

float const tabla[7] = {2.365911325937548E-9,-7.586309854875297E-7,9.497281340070148E-5,-0.00574191399808,0.17229156237910,-2.12031069662595,18.34322067641318};

/******************** CONSTANTE DEL CONTROL DE MOTORES ********************/

#define MOTOR_ESQ_PWM2 PIN_C1
#define MOTOR_DRET_PWM1 PIN_C2 //no se utiliza

#define MOTOR_ESQ_DIR PIN_E1
#define MOTOR_DRET_DIR PIN_E2
#define ENCODER_DRET PIN_B6
#define ENCODER_ESQ PIN_B7
/*************************************************************************/
/*************************************************************************/


/**************************************************************************/
/******************** DECLARACION DE PROTOTIPO ****************************/
/**************************************************************************/

/***************************** CAMARA *************************************/

void inicializar_camara();
void configuracion_puertos(); //Configura los puertos I/O
void tiempo_exposicion(int mseg);  //Tiempo de exposicion de la luz en la camara 
void resetear_camara();
void activar_camara();
void desactivar_camara();
unsigned int leer_pixel(int x,int y);
void capturar_por_filas(int modo);
void capturar_fila(int y,int modo);
void estabilizacion_imagen();
void histograma(int modo);
void localizacion();
void breakpoint();
void foto();
void foto_seg();
void calcula_distancia();
void calcula_ancho();
void calcula_angulo();

/***************************** CONTROL*************************************/

void inicializar_control();
void inicializar_vble_control();
void avanza_cm(long cm);
void retrocede_cm(long cm);
void movimiento_linial(long cm);
void movimiento_circular(long grados);
void motors_OFF();
void motors_ON(byte e);
void velocidad(long vel);
void derecha_grados(long grados);
void izquierda_grados(long grados);


/**************************************************************************/
/************************** VARIABLES GLOBALES ****************************/
/**************************************************************************/

/************************** CONTROL DE MOTORES ****************************/

long cont_motorI;
long cont_motorD;
short sem1 = 1;  // Semaforo de 1 bit
short sem2 = 1;  // Semaforo de 1 bit

byte estado = 0; // Robot parado

long pwmI = 0; // Robot a velocidad normal
long pwmD = 0; // Robot a velocidad normal
long pwmIS = 0; // Robot a velocidad lenta
long pwmDS = 0; // Robot a velocidad lenta

long speed = 0;
long incD = 0;
long incI = 0;

/***************************** CAMARA ************************************/

int contador;
int tiempo=70;
long pixelsleidos=0;
int min=255;
int segmentacio = 101;
int linea_ini = 0;
int pos_derecha = 0;
int pos_izquierda = 0;
int dcm;
short parat = 0;
int ancho = 0;
signed int angulo = 0;

/******************************** RS232 **********************************/

char orden = '#';

/*************************************************************************/
/*************************************************************************/


/**************************************************************************/
/********************* RUTINAS DE INTERRUPCION ****************************/
/**************************************************************************/

/************************ CONTADOR DE PASOS *******************************/ 
#int_rb
inter_rb()
{
int cont;

	if(input(ENCODER_DRET)) //Si es high B4 contar paso motor izquierda
	{
		if(sem1)
		{
			cont_motorI++; // Incremento contador vueltas
			incI++;
		}
		sem1 = 0; //semaforo rojo	
	}
	else
	{
		sem1 = 1; //semaforo verde
	}
	
	if(input(ENCODER_ESQ))
	{
		if(sem2)
		{
			cont_motorD++;
			incD++;
		}
		sem2 = 0; //semaforo rojo
	}
	else
	{
		sem2 = 1; //semaforo verde
	}
	
	if ((incI > 10) && estado)
	{
		if(estado == 1)
		{
			if(incI > incD)
				for(cont = 0;cont < 20;cont++)	
					pwmD = pwmD + (incI - incD);		
			if(incI < incD)
				for(cont = 0;cont < 20;cont++)	
					pwmD = pwmD - (incD - incI);
			if (pwmD > 1023)
				pwmD = 1023;
			set_pwm1_duty(pwmD);
		}
		else
		{		
			if(incI > incD)
				for(cont = 0;cont < 10;cont++)	
					pwmDS = pwmDS + (incI - incD);
			if(incI < incD)
				for(cont = 0;cont < 10;cont++)	
					pwmDS = pwmDS - (incD - incI);
			if (pwmDS > 1023)
				pwmDS = 1023;
			set_pwm1_duty(pwmDS);
		}
		incD = 0;
		incI = 0;
	}
}

/***************************** CAPTURA DE IMAGEN ***************************/

#int_rtcc                          // This function is called every time
clock_isr() {                      // the RTCC (timer0) overflows (255->0).
        
if(contador==28)
{                  // 2,78 veces por segundo
	if(pixelsleidos > 1000) 
	{
		if(min < 70)
	 	{
			if(tiempo > 1) tiempo=tiempo-1;
		}
		if(min > 80)
		{
 			if(tiempo < 100) tiempo=tiempo+1;
		}
		pixelsleidos=0;
		min=255;
	}
	resetear_camara();    //Reseteamos la camara, carga de condensadores 1ms
	tiempo_exposicion(tiempo); //Entrada de luz de 2ms
	contador=0;
}
else contador++;

}

/******************************** RS232 **********************************/

#int_rda
void serial_isr() {
static char buffer1,buffer2;

	buffer2 = buffer1;
	buffer1 = getc();
	if(buffer1 == '\n') orden = buffer2;

	switch(orden)
	{
		case 'S': printf(" %U",segmentacio);
			  break;
		case 'T': printf(" %U",tiempo);
			  break;
		case 'L': printf(" %U",linea_ini);
			  break;
		case 'O': calcula_ancho();
			  printf(" %U",ancho);
			  break;
		case 'A': calcula_angulo();
			  printf(" %D",angulo);
			  break;
		case 'D': calcula_distancia();
			  printf(" %U",dcm);
		          break;
		case 'Q': parat = 0;
			  break;
		case 'W': parat = 1;
			  break; 
	}

}

/*************************************************************************/
/*************************************************************************/


/*************************************************************************/
/******************************* MAIN ************************************/
/*************************************************************************/

void main() 
{
	configuracion_puertos();
	inicializar_camara();
	inicializar_control();
	activar_camara();
	estabilizacion_imagen();
	//histograma(0);	// Normalmente no hace falta

	velocidad(50);

	while(TRUE)
	{
		breakpoint();

		calcula_distancia();       
		calcula_angulo();

		if(angulo>0)
			derecha_grados(angulo);	
		else 
			izquierda_grados(-angulo);	

		delay_ms(1000);
		if((dcm > 5) && (dcm < 100))
			avanza_cm(dcm-5);
		delay_ms(1000);
	
	}
	
}


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

/*************************** INICIALIZACION ******************************/

void configuracion_puertos()
{

	//bit         XX543210
	set_tris_a( 0b11001011);	// 1 -input / 0 -output   
	//RA0=A/D, RA1=A/D, RA2=OUT, RA3=A/D, RA4=X, RA5=F0, RA6=X, RA7=X.

	//bit         76543210
	set_tris_b( 0b11000000);	// 1 -input / 0 -output   
	//RB0=X, RB1=ENABLE, RB2=C2, RB3=DES, RB4=X, RB5=X, RB6=X, RB7=X.

	//bit         76543210
	set_tris_c( 0b11001001);	// 1 -input / 0 -output   
	//RC0=X, RC1=X, RC2=X, RC3=C4, RC4=RESOUT, RC5=F4, RC6=X, RC7=X.

	//bit         76543210
	set_tris_d( 0b00000011);	// 1 -input / 0 -output   
	//RD0=F1, RD1=C3, RD2=F6, RD3=F5, RD4=F3, RD5=C0, RD6=RES, RD7=C1.

	//bit         XXXXX210
	set_tris_e( 0b11101000);	// 1 -input / 0 -output   
	//RE0=C6, RE1=C5, RE2=F2, RE3=X, RE4=X, RE5=X, RE6=X, RE7=X.

}


void inicializar_camara()
{
	 desactivar_camara();	 
	 output_high(RES); 
 	 output_low(RESOUT); 
 	 output_low(DES); 

	 // ADC
	 setup_adc_ports(RA0_RA1_RA3_ANALOG);
	 setup_adc(ADC_CLOCK_DIV_32);
	 set_adc_channel(0); 

	 // TIMER
	 set_rtcc(0);
   	 setup_counters( RTCC_INTERNAL, RTCC_DIV_256); // (20000000/(4*256*256))
   	 enable_interrupts(RTCC_ZERO);		       // 76 veces por segundo 	
	 enable_interrupts(INT_RDA);
   	 enable_interrupts(GLOBAL);
}

void inicializar_control()
{
	enable_interrupts(INT_RB);
	enable_interrupts(GLOBAL);

	setup_ccp1(CCP_PWM);   // Configure CCP1 as a PWM
	setup_ccp2(CCP_PWM);   // Configure CCP1 as a PWM
	setup_timer_2(T2_DIV_BY_1, 255, 1);
	motors_OFF();

	inicializar_vble_control();
	delay_ms(500);
}

void inicializar_vble_control()
{
	sem1 = 1;
	sem2 = 1;
	cont_motorI = 0;
	cont_motorD = 0;
	incD = 0;
	incI = 0;
}

/*************************** CAMARA **************************************/

void estabilizacion_imagen()
{
int x,y,z;
	for(z=0;z<10;z++)
	{
		  for(y=0;y<128;y++)
  		  	for(x=0;x<128;x++)
	  			leer_pixel(x,y);

	}
}

void tiempo_exposicion(int mseg)
{
	output_high(DES);
	delay_ms(mseg);
	output_low(DES);
}

void resetear_camara()
{
	output_low(RES);
	delay_us(250);
	output_high(RES);
	delay_us(10); //Tiempo de estabilización
}


void activar_camara()
{
	output_high(ENABLE);
}

void desactivar_camara()
{
	output_low(ENABLE);
}

unsigned int leer_pixel(int x,int y)
{

	int result;
	//									   |
	// Suponemos que los bits de seleccion de la camara van de la siguiente	  -|------ > X 
	// forma:  f6 f5 f4 f3 f2 f1 f0 - c6 c5 c4 c3 c2 c1 c0
	// Pixel 1 => 0000001 - 0000001

	// FILAS
	output_bit(COL0,bit_test(x,0));
	output_bit(COL1,bit_test(x,1));
	output_bit(COL2,bit_test(x,2));
	output_bit(COL3,bit_test(x,3));
	output_bit(COL4,bit_test(x,4));
	output_bit(COL5,bit_test(x,5));
	output_bit(COL6,bit_test(x,6));

	//COLUMNAS
	output_bit(FIL0,bit_test(y,0));
	output_bit(FIL1,bit_test(y,1));
	output_bit(FIL2,bit_test(y,2));
	output_bit(FIL3,bit_test(y,3));
	output_bit(FIL4,bit_test(y,4));
	output_bit(FIL5,bit_test(y,5));
	output_bit(FIL6,bit_test(y,6));


	desactivar_camara();  //Ponemos enable a 0 para poder hacer resout	 Y ^
	output_high(RESOUT);  //Activamos RESOUT	
	delay_us(1);        //						   |  
	output_low(RESOUT);   //Desactivamos RESOUT                                |
	activar_camara();  //Activamos la camara				   |

	// Leemos el valor analogico del pixel y retornamos
	result=read_adc();

	pixelsleidos++;

	if(result<min) min=result;

	return(result);
}

void foto()
{
	capturar_por_filas(0);
}

void foto_seg()
{
	capturar_por_filas(1);
}

void capturar_por_filas(int modo)
{
  int i;

  for(i=0;i<128;i++)
	capturar_fila(i,modo);
  
}

void capturar_fila(int y,int modo)
{
 int x;
 int salida;

 
 // Lectura pixel a pixel


  for(x=0;x<128;x++)
  {
	salida = leer_pixel(x,y);
	if(modo)
	{
	 	if(salida > segmentacio)
			printf("099 ");
		else 
			printf("020 ");	
	}
	else 
		printf("%03U ",salida);
	
  }
 
}


void histograma(int modo)
{

int x,y,salida;
unsigned long histo[20]; 
short max,min;


  for(x=0;x<20;x++) histo[x]=0;

  for(y=0;y<128;y++)
  {
	  for(x=0;x<128;x++)
	  {
		salida = leer_pixel(x,y);
		if(salida >= 90 && salida < 110)
		{
			histo[salida-90]++;
		}
		
	  }
  }
  
  if(modo) 
	for(x=0;x<20;x++) printf("%05lu ",histo[x]);

  max = 0;
  min = 0;
 
  x = 19;
  while((x > 1) && !max)
  {
  	if(histo[x] < histo[x-1] && histo[x-1] > histo[x-2] && histo[x-1] > 5)
		max = 1;
	x--; 
  } 

  while((x > 1) && !min  && max)
  {
	if(histo[x] > histo[x-1] && histo[x-1] <= histo[x-2])
		min = 1;
	x--;
  }

  if(min) segmentacio = x+90;

}


void localizacion()
{
int x,y,salida;
int suma_pixels = 0;

	for(y=0;(y<128) && (suma_pixels < 3);y++)
	{
		suma_pixels=0;
		for(x=0;x<128;x++)
		{
			salida = leer_pixel(x,y);
			if(salida > segmentacio) suma_pixels++;
		}
	}	
	if(suma_pixels >= 3) 
	{
		linea_ini = y-1;
		suma_pixels = 0;
		for(x=0;(x<128) && (suma_pixels < 3);x++)
		{
			suma_pixels = 0;
			for(y=127;y>linea_ini;y--)
			{
				salida = leer_pixel(x,y);
				if(salida > segmentacio) suma_pixels++;
			}
		}	

		if(suma_pixels >= 3) pos_izquierda = x-1;  
	
		suma_pixels = 0;
		for(x=127;(x>=0) && (suma_pixels < 3);x--)
		{
			suma_pixels = 0;
			for(y=127;y>linea_ini;y--)
			{
				salida = leer_pixel(x,y);
				if(salida > segmentacio) suma_pixels++;
			}
		}	
	
		if(suma_pixels > 3) pos_derecha = x+1;  
	}
	else // Si no ve nada
	{
		linea_ini = 127;
		pos_derecha = 63;
		pos_izquierda = 63;
	}
}
	
		
void breakpoint()
{

	while(parat)
	{
		if(orden == 'H')
		{ 
			histograma(1);
			orden = '#';
		}	

		if(orden == 'F')
		{
			foto();
			orden = '#';
		}
		if(orden == 'X') 
		{
			foto_seg(); 
			orden ='#';
		}
	}

}


void calcula_distancia()
{
float dist;
float x;
float y;

	localizacion(); 
	if(linea_ini > 4)
	{
		x = (float)linea_ini;

		dist = tabla[6] + tabla[5] * x; 
		y = x * x; 

		dist = dist + tabla[4] * y;
		y = y * x;

		dist = dist + tabla[3] * y;
		y = y * x;

		dist = dist + tabla[2] * y;
		y = y * x;

		dist = dist + tabla[1] * y;
		y = y * x;

		dist = dist + tabla[0] * y;

		dcm = (int)dist;
	}
	else
		dcm = 0;

}

void calcula_ancho()
{
	if(pos_derecha>=pos_izquierda)
		ancho = ((long)(pos_derecha+1)-(long)(pos_izquierda-1))*(long)dcm*0.006;
}

void calcula_angulo()
{

	angulo = (0.33*((signed int)pos_izquierda+(((signed int)pos_derecha-(signed int)pos_izquierda)/2)))-21;
	if(pos_izquierda+((pos_derecha-pos_izquierda)/2) < 63 ) 
		angulo = -angulo; 

}

/*************************** CONTROL **************************************/

void derecha_grados(long grados)
{
	output_high(MOTOR_DRET_DIR);  
	output_low(MOTOR_ESQ_DIR);

	movimiento_circular(grados);
}

void izquierda_grados(long grados)
{
	output_low(MOTOR_DRET_DIR);  
	output_high(MOTOR_ESQ_DIR);

	movimiento_circular(grados);
}

void motors_ON(byte e)
{
	estado = e;
	if(estado == 1)
	{
		pwmD = 1023-(100 - speed)*5;
		pwmI = 1023-(100 - speed)*5;

		set_pwm1_duty(pwmD);
		set_pwm2_duty(pwmI);
	}
	else
	{
		pwmDS = 1023-(100 - 30)*5;
		pwmIS = 1023-(100 - 30)*5;

		set_pwm1_duty(pwmDS);
		set_pwm2_duty(pwmIS);
	}
}

void motors_OFF()
{
	set_pwm1_duty(0);
	set_pwm2_duty(0);
	estado = 0;
}


void velocidad(long vel)
{
	speed = vel;
}

void movimiento_circular(long grados)
{
long dist;
long dist_rap;
long aux;

	inicializar_vble_control();
	
	dist = grados * 22;

	if(dist > 500) // 5 cm
	{
		motors_ON(1); // Rápido
		dist_rap = dist - 500;

		aux=cont_motorI*12 + 150;			

		while(aux < dist_rap)
			aux=cont_motorI*12 + 150;			
	}
	motors_ON(2); // Lento

	aux=cont_motorI*12 + 150;			

	while(aux < dist)
		aux=cont_motorI*12 + 150;			

	// 31/3 = 10
	// Cuando uno de los dos contadores llegue a cm saldrá del bucle

	motors_OFF();


}

void movimiento_lineal(long cm)
{
long dist;
long dist_rap;
long aux;

	inicializar_vble_control();
	
	dist = cm * 100;

	if(dist > 500) // 5 cm
	{
		motors_ON(1); // Rápido
		dist_rap = dist - 500;

		aux=cont_motorI*12 + 150;			

		while(aux < dist_rap)
			aux=cont_motorI*12 + 150;			
	}
	motors_ON(2); // Lento

	aux=cont_motorI*12 + 150;			

	while(aux < dist)
		aux=cont_motorI*12 + 150;			

	// 31/3 = 10
	// Cuando uno de los dos contadores llegue a cm saldrá del bucle

	motors_OFF();

}

void avanza_cm(long cm)
{
	output_high(MOTOR_DRET_DIR);  
	output_high(MOTOR_ESQ_DIR);

	movimiento_lineal(cm);
}

void retrocede_cm(long cm)
{
	output_low(MOTOR_DRET_DIR);  
	output_low(MOTOR_ESQ_DIR);
	
	movimiento_lineal(cm);
}


/*************************************************************************/
/******************************* FIN *************************************/
/*************************************************************************/