#include "homing.h"

#define sentido_motor1 1
#define sentido_motor2 1

volatile homing_t homing;

void Homing(void){
	uint16_t i;
	
	homing.state = 1;
	
	NVIC_DisableIRQ(EXTI4_IRQn);
	NVIC_DisableIRQ(EXTI9_5_IRQn);
	
	// Reseteamos valor del switch
	homing.B0 = 0;
	homing.B1 = 0;
	homing.B2 = 0;
	homing.B3 = 0;
	homing.Index1e=0;
	homing.Index1m=0;
	homing.Index2e=0;
	homing.Index2m=0;
	
	//------------------------------------- ESLABÓN 1 ------------------------------------------------
		
	// Buscamos el bumper 1	
	while(homing.B0==0)
	{	
		DAC->DHR12R1 = escalado(2.9)*sentido_motor1;   // lo de sentido_motor1 es 1 cambiar en la definicion a -1 si es sentido contrario
	}
	// Ahora tenemos que buscar el índice para ello habilitamos las interrupciones del pin al que este conectado el Index del encoder
	Delayms(1000);
	NVIC_EnableIRQ(EXTI4_IRQn);
	
	while(homing.Index1e==0){
		DAC->DHR12R1 = escalado(2.1)*sentido_motor1;  // movemos en dirección contraria en busqueda del primer indice
	}
	// una vez encontrado se para el motor y se registra la posición de theta 1 (esto se hace directamente en la interrupción)
	DAC->DHR12R1 = escalado(2.5);  // por si acaso	
	NVIC_DisableIRQ(EXTI4_IRQn);  // Deshabilitamos las interrupciones que causa el Index1 (ya tenemos lo que queríamos)

	NVIC_EnableIRQ(EXTI9_5_IRQn);
	
	while(homing.Index1m==0){
		DAC->DHR12R1 = escalado(2.9)*sentido_motor1;  // movemos en dirección contraria en busqueda del segundo indice
	}
	// una vez encontrado se para el motor y se registra la posición de theta 1 (esto se hace directamente en la interrupción)
	DAC->DHR12R1 = escalado(2.5);  // por si acaso	
	NVIC_DisableIRQ(EXTI9_5_IRQn);  // Deshabilitamos las interrupciones que causa el Index1 (ya tenemos lo que queríamos)	
	
	Delayms(2000);
	
	//------------------------------------- ESLABÓN 2 ------------------------------------------------
	
	// Buscamos el bumper 2	
	while(homing.B1==0)
	{	
		DAC->DHR12R2 = escalado(3.0)*sentido_motor2;
	}
	// Ahora tenemos que buscar el índice para ello habilitamos las interrupciones del pin al que este conectado el Index del encoder
	NVIC_EnableIRQ(EXTI9_5_IRQn);
	while(homing.Index2e==0){
		DAC->DHR12R2 = escalado(2.1)*sentido_motor2;  // movemos en dirección contraria en busqueda del primer indice
	}
	// una vez encontrado se para el motor y se registra la posición de theta 1 (esto se hace directamente en la interrupción)
	DAC->DHR12R2 = escalado(2.5);  // por si acaso	
	NVIC_DisableIRQ(EXTI9_5_IRQn);  // Deshabilitamos las interrupciones que causa el Index1 (ya tenemos lo que queríamos)	
	
	Delayms(2000);
	
	// Deshabilitamos modo homing
	
	homing.state=0;
	
}


