/* Include core modules */
#include "stm32f4xx_gpio.h"
#include "stm32f4xx_dac.h"
#include <stm32f4xx_it.h>
#include "stm32f4xx.h"

// TM Library
#include "tm_stm32f4_delay.h"
#include "tm_stm32f4_disco.h"
#include "tm_stm32f4_adc.h"
#include "tm_stm32f4_rng.h"
#include "tm_stm32f4_sdram.h"
#include "tm_stm32f4_usb_vcp.h"

// C Library
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <stdint.h>	//clock
#include <stdlib.h>

// User Library
#include "defines.h"
#include "paradacontrolada.h"
#include <homing.h>
#include <config.h>
#include <trapez.h>
#include "functions.h"
#include "controller.h"

//Señal senoidal
#define Amplitude 2
#define Offset 2.5
#define Frec 0.25

#define Reductor 70

// Elegir modo Identificación/ Tipo Control
#define control
//#define obtain_data_VCP
#define PD
//#define PDG
//#define LQR_T

extern volatile int8_t flag_Exp;

float Read1, Read2, Read3, Read4, Read5;
float Rtiempo, tension, Rtension, sensor1, sensor2, sensor3, noise;
float tau_elast, tau_elast_ref, k, Je, be;
float u1, u2, pos_ref1, pos_ref2;

float K[2][8][348];
float Kv[2][8][348];
float v[8][349];


//Se declaran dos encoders con la estructura encoder
volatile encoder encoder1, encoder2, encoder3, encoder4;	

// FUNCIONES CREADAS - CONFIGURACIONES
int32_t escalado(float value);
//void Pin_Start_Motor(void);

// PROGRAMA PRINCIPAL
int main(void) {
	float time;
	
	trapezoidal my_trajectory1, my_trajectory2;
	robot robot_2DOF;
	
    uint32_t i = 0, t = 0;
	uint32_t aux = 0;
	char cadena;
	//float tau_elast_referencia;	
	float tsampling = 0.001;
	time=0;
	
	// Datos brazo robot para control PD + G
	robot_2DOF.d1 = 0.25;
	robot_2DOF.d2 = 0.25;
	robot_2DOF.L1 = 0.5;
	robot_2DOF.L2 = 0.5;
	robot_2DOF.m1 = 10;
	robot_2DOF.m2 = 5;
	robot_2DOF.mr2 = 2;
	
	// Condiciones iniciales
	encoder1.posEncoder_PPR = 0;
	encoder2.posEncoder_PPR = 0;
	encoder3.posEncoder_PPR = 0;
	encoder4.posEncoder_PPR = 0;
	encoder1.previous_posEncoder_PPR = 0;
	encoder2.previous_posEncoder_PPR = 0;
	encoder3.previous_posEncoder_PPR = 0;
	encoder4.previous_posEncoder_PPR = 0;
		
	//SystemInit();
	
	// ENCODER INIT
	encoder1_config(); // PD12 y PD13   TIM4
	encoder2_config(); // PA8 y PA9 	TIM1
	encoder3_config(); // PD12 y PD13	TIM8
	encoder4_config(); // PA8 y PA9		TIM3
	 //LOS ENCODERS DAN 2000 PULSOS POR VUELTA!!!!!!!!!!!!!!!!
	
	// DAC INIT
	dac_init();   //PA4 /PA5
	
    DAC->DHR12R1 = escalado(2.5);   // motores parados 2.5 V entrada EPOS
	
	// DELAY FUNCTION INIT
	TM_DELAY_Init();
	
	#ifdef obtain_data_VCP
	// USB VCP INIT
	TM_USB_VCP_Init();
	#endif
	
	// LED INIT
    TM_DISCO_LedInit();
    
	// SDRAM INIT
    TM_SDRAM_Init();
	
	TM_DISCO_LedOn(LED_RED);
	
	// ADC INIT
	//TM_ADC_Init(ADC3, ADC_Channel_4); //PF6
	//TM_ADC_Init(ADC3, ADC_Channel_5); //PF7
	//TM_ADC_Init(ADC3, ADC_Channel_6); //PF8
		
	//GPIO_SetBits(GPIOG, GPIO_Pin_13);
	
	//Delayms(5000);
	
	TM_DISCO_LedOff(LED_RED);
	
	// PERIODO BUCLE DE CONTROL
	Control_TimerInit(1000);  //  Control de 0.1 ms
	
	for(i=0;i<5;i++){   			//parece que PB6 PB7 estan en conficto con otros perifericos y cosas del VCP
		Configure_GPIOB_EXTI(i);
	}
	
	/*for(i=0;i<3;i++){   			
		Configure_GPIOF_EXTI(i);
	}*/
	
	Delayms(5000);
	
	//############################# Generaración de trayectorias ########################################
		
	trapez4( 0, 1, 0.5, 2, 10, 500, 0.01,1, &my_trajectory1);
	trapez4( 0, 1, 0.5, 2, 10, 500, 0.01,1, &my_trajectory2);
	
	//######################################## HOMMING ##################################################
	
	//Homing();
	
	// Prueba indices
	NVIC_EnableIRQ(EXTI4_IRQn);
	NVIC_EnableIRQ(EXTI9_5_IRQn);
	DAC->DHR12R1 = escalado(3.0);
	while(1);
	//*/
	//################################## BUCLE DE CONTROL ###############################################
	TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
	TM_DISCO_LedOn(LED_GREEN);
	while(1){ 																	// for(i=0;i<=30000;i++)
		flag_Exp = 0;
		time = t*tsampling;
		
		// Lectura de la posición del encoder en pulsos por revolución
		encoder1.posEncoder_PPR = TIM_GetCounter(TIM4); //- encoder1.offset;  	// Lectura del encoder 1 - q1
		encoder2.posEncoder_PPR = TIM_GetCounter(TIM1); //- encoder2.offset;		// Lectura del encoder 2 - q2
		encoder3.posEncoder_PPR = TIM_GetCounter(TIM8);//- encoder3.offset;  	// Lectura del encoder 1 - theta1
		encoder4.posEncoder_PPR = TIM_GetCounter(TIM3); //- encoder4.offset;		// Lectura del encoder 2 - theta2
		

		// Conversion de la posicion de PPR a radianes
		encoder1.posEncoder_rad = PPR_to_Rad(encoder1.posEncoder_PPR, encoder1.previous_posEncoder_PPR);
		encoder2.posEncoder_rad = PPR_to_Rad(encoder2.posEncoder_PPR, encoder2.previous_posEncoder_PPR);
		encoder3.posEncoder_rad = (PPR_to_Rad(encoder3.posEncoder_PPR, encoder3.previous_posEncoder_PPR)) / Reductor;
		encoder4.posEncoder_rad = (PPR_to_Rad(encoder4.posEncoder_PPR, encoder4.previous_posEncoder_PPR)) / Reductor;
		
		#ifdef obtain_data_VCP
		
		// Almacenar datos SDRAM para despues enviar TeraTerm
		TM_SDRAM_WriteFloat(aux, encoder1.posEncoder_rad);
		TM_SDRAM_WriteFloat(aux+8, encoder2.posEncoder_rad);
		TM_SDRAM_WriteFloat(aux+16, encoder1.velEncoder_rad);
		TM_SDRAM_WriteFloat(aux+24, encoder2.velEncoder_rad);
		TM_SDRAM_WriteFloat(aux+32, time);
		aux += 40;
		
		#endif	
		
		// ESTIMAMOS LA VELOCIDAD VELOCIDAD
		if(time>0){
			encoder1.velEncoder_rad = speed_estimation(encoder1.posEncoder_rad, encoder1.previous_posEncoder_rad, tsampling); 	//(rad/s)
			encoder2.velEncoder_rad = speed_estimation(encoder2.posEncoder_rad, encoder2.previous_posEncoder_rad, tsampling);	//(rad/s)
			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)
		}
							
		// ######################### IDENTIFICACION ######################
		#ifndef control
			noise = (TM_RNG_Get()%Amplitude)*0.1;
			tension = (Offset + Amplitude * sin(2*Pi*Frec*time)) + noise;
			DAC->DHR12R1 = escalado(tension);
		#endif
		
		// ############################ CONTROL PD ##########################		
		#ifdef PD
		
			if(t<=my_trajectory1.size){
				pos_ref1 = my_trajectory1.position[t];
			}
			
			if(t<=my_trajectory2.size){
				pos_ref2 = my_trajectory2.position[t];
			}
			
			u1 = PD_action(encoder3.posEncoder_rad,encoder3.previous_velEncoder_rad ,pos_ref1);
			u2 = PD_action(encoder4.posEncoder_rad,encoder4.previous_velEncoder_rad, pos_ref2);
			
			DAC->DHR12R1 = escalado(u1);
			DAC->DHR12R2 = escalado(u2);
			
		#endif
		
		// ########################## CONTROL PD + G #########################	
			
		#ifdef PDG
		
			if(t<=my_trajectory1.size){
				pos_ref1 = my_trajectory1.position[t];
				g1 = (myrobot.m1*g*myrobot.d1+myrobot.mr2*g*myrobot.L1)*cos(pos_ref1)+myrobot.m2*g*(myrobot.L1*cos(pos_ref1)-myrobot.d1*(sin(pos_ref1)*sin(pos_ref1)-cos(pos_ref1)*cos(pos_ref1)));	
			}
			
			if(t<=my_trajectory2.size){
				pos_ref2 = my_trajectory2.position[t];
				g2 = myrobot.m2*g*myrobot.d1*(sin(pos_ref2)*sin(pos_ref2)-cos(pos_ref2)*cos(pos_ref2));
			}
			
			u1 = PD_action(encoder3.posEncoder_rad,encoder3.previous_velEncoder_rad ,pos_ref1) + g1;
			u2 = PD_action(encoder4.posEncoder_rad,encoder4.previous_velEncoder_rad, pos_ref2) + g2;
			
			DAC->DHR12R1 = escalado(u1);
			DAC->DHR12R2 = escalado(u2);
			
		#endif
		
		// #################### LQR + Trajectory ######################	
		#ifdef LQR_T
			
			u1 = encoder1.posEncoder_rad*K[1][1][t] + encoder2.posEncoder_rad*K[1][2][t] + encoder3.posEncoder_rad*K[1][3][t] + encoder4.posEncoder_rad*K[1][4][t] + encoder1.velEncoder_rad*K[1][5][t] + encoder2.velEncoder_rad*K[1][6][t] + encoder3.velEncoder_rad*K[1][7][t] + encoder4.velEncoder_rad*K[1][8][t]+ v[1][t]*Kv[1][1][t] + v[2][t]*Kv[1][2][t] + v[3][t]*Kv[1][3][t] + v[4][t]*Kv[1][4][t] + v[5][t]*Kv[1][5][t] + v[6][t]*Kv[1][6][t] + v[7][t]*Kv[1][7][t] + v[8][t]*Kv[1][8][t];
			u2 = encoder1.posEncoder_rad*K[2][1][t] + encoder2.posEncoder_rad*K[2][2][t] + encoder3.posEncoder_rad*K[2][3][t] + encoder4.posEncoder_rad*K[2][4][t] + encoder1.velEncoder_rad*K[2][5][t] + encoder2.velEncoder_rad*K[2][6][t] + encoder3.velEncoder_rad*K[2][7][t] + encoder4.velEncoder_rad*K[2][8][t]+ v[1][t]*Kv[2][1][t] + v[2][t]*Kv[2][2][t] + v[3][t]*Kv[2][3][t] + v[4][t]*Kv[2][4][t] + v[5][t]*Kv[2][5][t] + v[6][t]*Kv[2][6][t] + v[7][t]*Kv[2][7][t] + v[8][t]*Kv[2][8][t];
		
			DAC->DHR12R1 = escalado(u1);
			DAC->DHR12R2 = escalado(u2);
			
		#endif
			
		// ######################### END_CONTROL ######################
		
		// Almacenamos las posiciones para la siguiente iteración		
		encoder1.previous_posEncoder_PPR = encoder1.posEncoder_PPR;
		encoder2.previous_posEncoder_PPR = encoder2.posEncoder_PPR;
		encoder3.previous_posEncoder_PPR = encoder3.posEncoder_PPR;
		encoder4.previous_posEncoder_PPR = encoder4.posEncoder_PPR;
		
		encoder1.previous_posEncoder_rad = encoder1.posEncoder_rad;
		encoder2.previous_posEncoder_rad = encoder2.posEncoder_rad;
		encoder3.previous_posEncoder_rad = encoder3.posEncoder_rad;
		encoder4.previous_posEncoder_rad = encoder4.posEncoder_rad;
	
		while(flag_Exp != 1);
		
		t++;
	}
			
	TM_DISCO_LedOff(LED_GREEN);
	TIM_ITConfig(TIM2, TIM_IT_Update, DISABLE);
	TM_DISCO_LedOn(LED_RED);
	
	aux = 0;
 
	#ifdef obtain_data_VCP
	// Envio datos teraterm
	if (TM_USB_VCP_GetStatus() == TM_USB_VCP_CONNECTED) {
		
		TM_DISCO_LedOn(LED_GREEN);
		TM_DISCO_LedOff(LED_RED);
		
		TM_USB_VCP_Puts("A = [");
	
		for (i = 0; i < 30000; i++) {
			Read1 = TM_SDRAM_ReadFloat(aux);
			Read2 = TM_SDRAM_ReadFloat(aux+8);
			Read3 = TM_SDRAM_ReadFloat(aux+16);
			Read4 = TM_SDRAM_ReadFloat(aux+24);
			Read5 = TM_SDRAM_ReadFloat(aux+32);
			aux += 40;
			
			sprintf(&cadena,"%f %f %f %f %f  \r\n",Read1, Read2, Read3, Read4, Read5); 
			//printf("%d %d %f %f \r\n", Read, Read2, Read3, Read4);
			TM_USB_VCP_Puts(&cadena);
			Delayms(5);
		}
		TM_USB_VCP_Puts("];");
    }
	
	#endif
	
    TM_DISCO_LedOn(LED_RED);
	
	#ifdef control
	free(my_trajectory1.position);
	free(my_trajectory1.aceleration);
	//free(my_trajectory1.der_jerk);
	//free(my_trajectory1.jerk);
	free(my_trajectory1.velocity);
	#endif
	
    while(1);
} 

