//#include "defines.h"
#include "controller.h"


// GANANCIAS DEL CONTROLADOR PI
#define Kp 1
#define Ki 1
#define Kd 1

//DEFINIMOS ACCIONES DE CONTROL (SATURACIÓN), EPOS TENSIÓN DE ENTRADA ENTRE 0V y 5V
#define PI_max 2.5
#define PI_min 0.0
#define PD_max 2.5
#define PD_min 0.0
#define V_offset 0.3

// CONTROLADOR PI
float PI_action(float vel_encoder, float vel_ref, float dt){
	float vel_error, vel_P, vel_I, vel_error_last, PI;

	/* Calculamos el error */
	vel_error = vel_ref - vel_encoder;
	/* Calculamos la acción del proporcional */
	vel_P = vel_error * Kp;
	
	/* Calculamos la acción integrativa */
	vel_I = vel_error_last + (vel_error * Ki * dt);
	/* Obtenemos la acción del PI */
	PI = vel_P  + vel_I;
	
	/* Nos aseguramos de que no sobrepase un límite (SATURACIÓN)*/
	if(PI>PI_max)PI = PI_max;
	else if(PI<(PI_min))PI = PI_min;
	
	/* Almaenamos el error atual, que utilizaremos en el siguiente cálculo */
	vel_error_last = vel_error;
	
	return PI;
}


// CONTROLADOR PD
float PD_action(float pos, float vel, float pos_ref){

	float pos_error, pos_error_last, acc_P, acc_D, PD;
	
	// Calculamos el error //
	pos_error = pos_ref - pos;
	// Calculamos la acción del proporcional ///
	acc_P = pos_error * Kp;	
	// Calculamos la acción derivativa //
	acc_D = vel * Kd;
	// Obtenemos la acción del PI //
	PD = acc_P - acc_D;	
	// Nos aseguramos de que no sobrepase un límite (SATURACIÓN)//
	if(PD>PD_max)PD = PD_max;
	else if(PD<(PD_min))PD = PD_min;	
	// Almaenamos el error atual, que utilizaremos en el siguiente cálculo //
	pos_error_last = pos_error;
	
	return PD;
}


/*PDG_struct PDG_action(float pos_encoder1, float pos_encoder2, float pos_ref1, float pos_ref2, float dt, robot myrobot){
	float g1, g2, pos_error;
	PDG_struct PDG;
	
	// Calculamos la acción a partir de la compensación de gravedad //
	g1 = (myrobot.m1*g*myrobot.d1+myrobot.mr2*g*myrobot.L1)*cos(pos_encoder1)+myrobot.m2*g*(myrobot.L1*cos(pos_encoder1)-myrobot.d1*(sin(pos_encoder1)*sin(pos_encoder2)-cos(pos_encoder1)*cos(pos_encoder2)));
	g2 = myrobot.m2*g*myrobot.d1*(sin(pos_encoder1)*sin(pos_encoder2)-cos(pos_encoder1)*cos(pos_encoder2));
	
	// Calculamos el error //
	PDG.pos_error[0] = pos_ref1 - pos_encoder1;
	PDG.pos_error[1] = pos_ref2 - pos_encoder2;
	// Calculamos la acción del proporcional //
	PDG.P_action[0] = PDG.pos_error[0] * PDG.K_p[0];
	PDG.P_action[1] = PDG.pos_error[1] * PDG.K_p[1];	
	// Calculamos la acción derivartiva //
	PDG.D_action[0] = ((PDG.pos_error[0] - PDG.previous_pos_error[0]) * PDG.K_d[0])/dt;
	PDG.D_action[1] = ((PDG.pos_error[1] - PDG.previous_pos_error[1]) * PDG.K_d[1])/dt;
	// Obtenemos la acción del PI //
	PDG.PD_action[0] = PDG.P_action[0] - PDG.D_action[0] + g1;	
	PDG.PD_action[1] = PDG.P_action[1] - PDG.D_action[1] + g2;	
	// Nos aseguramos de que no sobrepase un límite (SATURACIÓN)//
	if(PDG.PD_action[0]>PD_max)PD = PD_max;
	else if(PDG.PD_action[0]<(PD_min))PD = PD_min;	
	// Almaenamos el error atual, que utilizaremos en el siguiente cálculo //
	PDG.previous_pos_error[0] = PDG.pos_error[0];
	
	return PDG;
}*/

