#include "paradacontrolada.h"

#define V_offset 0.3
#define deceleration 3

extern volatile encoder encoder1, encoder2, encoder3, encoder4;
extern volatile int8_t flag_Exp;

float vel_ref1, vel_ref2, vel_max1, vel_max2, time, tsampling = 0.001;


void paradacontrolada(void){
	
	uint32_t t = 0;
	float V1_ref=0, V2_ref=0;
	//char cadena;
		
	//HABILITAMOS INTERRUPCIONES(para bucle de control), CADA 1 ms
	TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
	
	while(1){ 					
		flag_Exp = 0;
		time = t*tsampling;
		
		if(t==0){
			vel_max1 = encoder3.posEncoder_rad;
			vel_max2 = encoder4.posEncoder_rad;
		}
						
		//GENERA LA VELOCIDAD DE REFERENCIA (rampa) EN FUNCIÓN DEL TIEMPO Y PARAMETRO DE DESACELERACIÓN DEFINIDO ANTERIORMENTE
		V1_ref = vel_reference(vel_max1, deceleration, time);
		V2_ref = vel_reference(vel_max2, deceleration, time);		
		
		// Lectura de la posición del encoder en pulsos por revolución
		encoder3.posEncoder_PPR = TIM_GetCounter(TIM8);// - encoder3.offset;  	// Lectura del encoder 3 theta1
		encoder4.posEncoder_PPR = TIM_GetCounter(TIM3);// - encoder4.offset;		// Lectura del encoder 4 theta2
		
		// Conversion de la posicion de PPR a radianes
		encoder3.posEncoder_rad = PPR_to_Rad(encoder3.posEncoder_PPR, encoder3.previous_posEncoder_PPR);
		encoder4.posEncoder_rad = PPR_to_Rad(encoder4.posEncoder_PPR, encoder4.previous_posEncoder_PPR);
		
		// VELOCIDAD ACTUAL
		//											pos.actual					pos.anterior					tiempo
		encoder3.velEncoder_rad = speed_estimation(encoder3.posEncoder_rad, encoder3.previous_posEncoder_rad, tsampling); 	//(rad/s)
		encoder4.velEncoder_rad = speed_estimation(encoder4.posEncoder_rad, encoder4.previous_posEncoder_rad, tsampling);	//(rad/s)
		
		
		// ENVIAR DATOS TERATERM
		/*if(t % 500==0){
			sprintf(&cadena,"%f %f %f %f \r\n",encoder3.velEncoder_rad, encoder4.velEncoder_rad, vel_E1, V1_ref); 
				//printf("%d %d %f %f \r\n", Read, Read2, Rtension, Rtiempo);
			TM_USB_VCP_Puts(&cadena);
		}*/

	
		//APLICA LA ACCIÓN DE CONTROL (para la acciónd e control se neceista la velocidad de la prealimentación del controlador PI)
		DAC->DHR12R1 = escalado(PI_action(encoder3.velEncoder_rad, V1_ref, tsampling));
		DAC->DHR12R2 = escalado(PI_action(encoder4.velEncoder_rad, V2_ref, tsampling));
		
		// ALMACENAMOS POSICIÓN ACTUAL PARA LA SIGUIENTE ITERACIÓN DE CALCULO DE VELOCIDAD
		encoder3.previous_posEncoder_PPR = encoder3.posEncoder_PPR;
		encoder4.previous_posEncoder_PPR = encoder4.posEncoder_PPR;
		
		// CONTROL DE TIEMPO DEL BUCLE
		while(flag_Exp != 1);
		t++;
	}

	TIM_ITConfig(TIM2, TIM_IT_Update, DISABLE);
}

