vmConfort_v6 - Esta versión v6 pasa a ser el nuevo master. Funci…

virtualmech » Code » vmConfort_v6

virtualmech / Mbed 2 deprecated vmConfort_v6

Esta versión v6 pasa a ser el nuevo master. Funciona correctamente

Dependencies: ADXL345 Display1602 MSCFileSystem SDFileSystem mbed FATFileSystem

vmConfort.cpp@2:cc4a43d806e2, 2014-06-06 (annotated)

Committer:: JuanManuelAmador
Date:: Fri Jun 06 08:52:28 2014 +0000
Revision:: 2:cc4a43d806e2
Parent:: 1:ff391cfd0e77

Correci?n del baudrate de 112500 a 115200

Who changed what in which revision?

User	Revision	Line number	New contents of line
jjmedina	0:a5367bd4e404	1	#include "mbed.h"
jjmedina	0:a5367bd4e404	2	#include "Display1602.h"
jjmedina	0:a5367bd4e404	3	//#include "SDFileSystem.h"
jjmedina	0:a5367bd4e404	4	#include "MSCFileSystem.h"
jjmedina	0:a5367bd4e404	5
jjmedina	0:a5367bd4e404	6	#include <iostream>
jjmedina	0:a5367bd4e404	7	#include <math.h>
jjmedina	0:a5367bd4e404	8	#include <stdio.h>
jjmedina	0:a5367bd4e404	9	#include <string.h>
jjmedina	0:a5367bd4e404	10	#include <list>
jjmedina	0:a5367bd4e404	11	#include <vector>
jjmedina	0:a5367bd4e404	12	#include <algorithm>
jjmedina	0:a5367bd4e404	13
jjmedina	0:a5367bd4e404	14	#include "filters.h"
jjmedina	0:a5367bd4e404	15	#include "BMA180.h"
jjmedina	0:a5367bd4e404	16	//#include "ADXL345.h"
jjmedina	0:a5367bd4e404	17	#include "RingBuffer.h"
jjmedina	0:a5367bd4e404	18
jjmedina	0:a5367bd4e404	19	using namespace std;
jjmedina	0:a5367bd4e404	20
jjmedina	0:a5367bd4e404	21
jjmedina	0:a5367bd4e404	22	Timer timer;
jjmedina	0:a5367bd4e404	23
jjmedina	0:a5367bd4e404	24	int TiempoInicio = 0;
jjmedina	0:a5367bd4e404	25	int TiempoFin = 0;
jjmedina	0:a5367bd4e404	26
jjmedina	0:a5367bd4e404	27	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	28	/// CONSTANTES DEPENDIENTES DE LA FRECUENCIA DE ADQUISICION (sus valores se establecen en ParamDependFrecAdq()) ///
jjmedina	0:a5367bd4e404	29	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	30
jjmedina	0:a5367bd4e404	31	unsigned int PasoTiempo; //Paso de tiempo de adquisicion en [us]. Frecuencia de adquisicion = 1/PasoTiemo
jjmedina	0:a5367bd4e404	32	float gain; // CONSTANTE DEL FILTRO DE BUTTERWORTH PASO BAJO 10 HZ
jjmedina	0:a5367bd4e404	33	float c_butt1; // CONSTANTE DEL FILTRO DE BUTTERWORTH PASO BAJO 10 HZ
jjmedina	0:a5367bd4e404	34	float c_butt2; // CONSTANTE DEL FILTRO DE BUTTERWORTH PASO BAJO 10 HZ
jjmedina	0:a5367bd4e404	35	unsigned int ndes; //Puntos que se descartan al principio por el Filtro Butterworth
jjmedina	0:a5367bd4e404	36	unsigned int nmed; //Puntos para calcular la media de aZ al principio del viaje despues de los puntos descartados
jjmedina	0:a5367bd4e404	37
jjmedina	0:a5367bd4e404	38	int nDatosPorPaso = 1;
jjmedina	0:a5367bd4e404	39
jjmedina	0:a5367bd4e404	40	/////////////////////////////////
jjmedina	0:a5367bd4e404	41	/// DISPOSITIVOS, PERIFERICOS ///
jjmedina	0:a5367bd4e404	42	/////////////////////////////////
jjmedina	0:a5367bd4e404	43
jjmedina	0:a5367bd4e404	44	/// ACELEROMETRO ANALOGICO ///
jjmedina	0:a5367bd4e404	45
jjmedina	0:a5367bd4e404	46	//AnalogIn Pin_aX(p15);
jjmedina	0:a5367bd4e404	47	//AnalogIn Pin_aY(p16);
jjmedina	0:a5367bd4e404	48	//AnalogIn Pin_aZ(p17);
jjmedina	0:a5367bd4e404	49
jjmedina	0:a5367bd4e404	50	/// ACELEROMETRO DIGITAL Bosch///
jjmedina	0:a5367bd4e404	51
jjmedina	0:a5367bd4e404	52	BMA180 bma180(p5,p6,p7,p8);
jjmedina	0:a5367bd4e404	53
jjmedina	0:a5367bd4e404	54	/// ACELEROMETRO DIGITAL AnalogDevices ADXL345///
jjmedina	0:a5367bd4e404	55
jjmedina	0:a5367bd4e404	56	//ADXL345 adxl345(p5, p6, p7, p8);
jjmedina	0:a5367bd4e404	57
jjmedina	0:a5367bd4e404	58	Serial pc(USBTX, USBRX); // tx, rx
jjmedina	0:a5367bd4e404	59
jjmedina	0:a5367bd4e404	60	/// TARJETA SD ///
jjmedina	0:a5367bd4e404	61
jjmedina	0:a5367bd4e404	62	//SDFileSystem sd(p11, p12, p13, p14, "local");
jjmedina	0:a5367bd4e404	63
jjmedina	0:a5367bd4e404	64	/// PEN DRIVE ///
jjmedina	0:a5367bd4e404	65
jjmedina	0:a5367bd4e404	66	MSCFileSystem msc("local");
jjmedina	0:a5367bd4e404	67
jjmedina	0:a5367bd4e404	68	/// MEMORIA FLASH LOCAL DE LA MBED ///
jjmedina	0:a5367bd4e404	69
jjmedina	0:a5367bd4e404	70	LocalFileSystem local("mbedFlash"); // Create the local filesystem under the name "local"
jjmedina	0:a5367bd4e404	71
jjmedina	0:a5367bd4e404	72	/// LEDS ///
jjmedina	0:a5367bd4e404	73
jjmedina	0:a5367bd4e404	74	DigitalOut red_LED(p30);
jjmedina	0:a5367bd4e404	75	DigitalOut green_LED(p29);
jjmedina	0:a5367bd4e404	76
jjmedina	0:a5367bd4e404	77	/// PUSH BUTTONS ///
jjmedina	0:a5367bd4e404	78
jjmedina	0:a5367bd4e404	79	DigitalOut red_PUSH(p9);
jjmedina	0:a5367bd4e404	80	DigitalOut green_PUSH(p10);
jjmedina	0:a5367bd4e404	81
jjmedina	0:a5367bd4e404	82	/// DISPLAY LCD ///
jjmedina	0:a5367bd4e404	83
jjmedina	0:a5367bd4e404	84	//Display1602 display(p18,p19,p20,p28,p27,p26,p25,p24,p23,p22,p21);
jjmedina	0:a5367bd4e404	85	Display1602 display(p18,p19,p20,p21,p22,p23,p24,p25,p26,p27,p28);
jjmedina	0:a5367bd4e404	86
jjmedina	0:a5367bd4e404	87	//void InterpolaMedidasPerdidas(unsigned int, unsigned int);
jjmedina	0:a5367bd4e404	88	void FiltradoPeriodosIntegracion(unsigned int, unsigned int);
jjmedina	0:a5367bd4e404	89	void DatosConfort(unsigned int, unsigned int, int);
jjmedina	0:a5367bd4e404	90	float RegresionJerk(float *, unsigned int);
jjmedina	0:a5367bd4e404	91	void calcula_n05n95_Boveloc_jerkmax(unsigned int , unsigned int, int );
jjmedina	0:a5367bd4e404	92	void calcula_A95_V_acel_decel(unsigned int, unsigned int, int);
jjmedina	0:a5367bd4e404	93	void vibraReadFilter(unsigned int , unsigned int );
jjmedina	0:a5367bd4e404	94
jjmedina	0:a5367bd4e404	95	void calculaCarNCar(unsigned int , unsigned int, int );
jjmedina	0:a5367bd4e404	96	void calculaStep2(unsigned int , unsigned int );
jjmedina	0:a5367bd4e404	97
jjmedina	0:a5367bd4e404	98	void calculaZXYvibweighted( unsigned int , unsigned int);
jjmedina	0:a5367bd4e404	99
jjmedina	0:a5367bd4e404	100	void DisplayResultados1(void);
jjmedina	0:a5367bd4e404	101
jjmedina	0:a5367bd4e404	102	void DisplayResultados2(void);
jjmedina	0:a5367bd4e404	103
jjmedina	0:a5367bd4e404	104	void FileResultados( unsigned int );
jjmedina	0:a5367bd4e404	105
jjmedina	0:a5367bd4e404	106	unsigned int nArr, nPar, nV0, nVfin; //Instantes (orden en el vector de datos) caracteristicos del viaje
jjmedina	0:a5367bd4e404	107	float t_Arr, t_V0, t_Vfin, t_Par; //Instantes (valores del tiempo) caracteristicos del viaje
jjmedina	0:a5367bd4e404	108	float vArr = 0.0; //Velocidad que se calcula por integración en el instante de arranque (debe ser 0)
jjmedina	0:a5367bd4e404	109	float vPar = 0.0; //Velocidad que se calcula por integración en el instante de parada (debe ser 0)
jjmedina	0:a5367bd4e404	110
jjmedina	0:a5367bd4e404	111	float vZmFin = 0.0; //Media de vZ al final del viaje (debe ser zero pero no lo es)
jjmedina	0:a5367bd4e404	112	float tFin; //Instante final
jjmedina	0:a5367bd4e404	113
jjmedina	0:a5367bd4e404	114	float aZm0 = 0.0; //Aceleracion media de referencia en instantes iniciales en dirección vertical Z
jjmedina	0:a5367bd4e404	115	float aYm0 = 0.0; //Aceleracion media de referencia en instantes iniciales en dirección lateral X
jjmedina	0:a5367bd4e404	116	float aXm0 = 0.0; //Aceleracion media de referencia en instantes iniciales en dirección lateral Y
jjmedina	0:a5367bd4e404	117
jjmedina	0:a5367bd4e404	118	//////////////////////////
jjmedina	0:a5367bd4e404	119	/// MEDIDAS EN DISPLAY ///
jjmedina	0:a5367bd4e404	120	//////////////////////////
jjmedina	0:a5367bd4e404	121
jjmedina	0:a5367bd4e404	122	float vNom = 0.0; //Velocidad nominal
jjmedina	0:a5367bd4e404	123	float amax = 0.0;
jjmedina	0:a5367bd4e404	124	float dmax = 0.0;
jjmedina	0:a5367bd4e404	125	float Vmax = 0.0;
jjmedina	0:a5367bd4e404	126	float jerkmax = 0.0;
jjmedina	0:a5367bd4e404	127	float VA95(0.0);
jjmedina	0:a5367bd4e404	128	float aA95(0.0);
jjmedina	0:a5367bd4e404	129	float dA95(0.0);
jjmedina	0:a5367bd4e404	130
jjmedina	0:a5367bd4e404	131	//por pantalla
jjmedina	0:a5367bd4e404	132	float max_peaktopeak_car_(0.0);
jjmedina	0:a5367bd4e404	133	float A95_peaktopeak_car_(0.0);
jjmedina	0:a5367bd4e404	134	float max_peaktopeak_ncar_(0.0);
jjmedina	0:a5367bd4e404	135	float max_peaktopeak_X_(0.0);
jjmedina	0:a5367bd4e404	136	float A95_peaktopeak_X_(0.0);
jjmedina	0:a5367bd4e404	137	float max_peaktopeak_Y_(0.0);
jjmedina	0:a5367bd4e404	138	float A95_peaktopeak_Y_(0.0);
jjmedina	0:a5367bd4e404	139
jjmedina	0:a5367bd4e404	140	float d_arr, d_par; //Desplazamientos medidos en instantes de arranque y de parada
jjmedina	0:a5367bd4e404	141
jjmedina	0:a5367bd4e404	142	unsigned int Bo0, Bo1, Bo2, Bo3; //Limites de los periodos de calculo de norma ISO
jjmedina	0:a5367bd4e404	143	unsigned int Boacel; //Instante donde deja de acelerar para empezar a decelerar
jjmedina	0:a5367bd4e404	144
jjmedina	0:a5367bd4e404	145	float Vmax1 = 0.0;
jjmedina	0:a5367bd4e404	146	float Vmax2 = 0.0;
jjmedina	0:a5367bd4e404	147	unsigned int n05Vmax1(0);unsigned int n95Vmax1(0);unsigned int n05Vmax2(0);unsigned int n95Vmax2(0);
jjmedina	0:a5367bd4e404	148	unsigned int Bo_veloc_acel(0);
jjmedina	0:a5367bd4e404	149	unsigned int Bo_veloc_decel(0);
jjmedina	0:a5367bd4e404	150
jjmedina	0:a5367bd4e404	151	int Nregionesconst(0);
jjmedina	0:a5367bd4e404	152	int Nregiones(0);
jjmedina	0:a5367bd4e404	153	float **region;
jjmedina	0:a5367bd4e404	154	float regionconst[20][4]; //50 porque no sabemos el numero de filas. RESOLVER MEJOR, JUAN 04-05-2012.
jjmedina	0:a5367bd4e404	155
jjmedina	0:a5367bd4e404	156	////////////////////////
jjmedina	0:a5367bd4e404	157	/// IDIOMA DE LA GUI ///
jjmedina	0:a5367bd4e404	158	////////////////////////
jjmedina	0:a5367bd4e404	159
jjmedina	0:a5367bd4e404	160	unsigned int TRADUCE = 0; // TRADUCE = 0 gui en español, TRADUCE = 1, traduce gui al inglés
jjmedina	0:a5367bd4e404	161	unsigned int SeleccionIdioma = 0; // Vale 1 cuando ya se ha seleccionado el idioma
jjmedina	0:a5367bd4e404	162
jjmedina	0:a5367bd4e404	163	/////////////////////////////////
jjmedina	0:a5367bd4e404	164	/// FRECUENCIA DE ADQUISICION ///
jjmedina	0:a5367bd4e404	165	/////////////////////////////////
jjmedina	0:a5367bd4e404	166
jjmedina	0:a5367bd4e404	167	unsigned int FrecAdq = 0; //La frecuencia de adquisición puede ser 320 Hz (FrecAdq = 320) o 1000 Hz (FrecAdq = 1000)
jjmedina	0:a5367bd4e404	168	unsigned int SeleccionFrecAdq = 0; // Vale 1 cuando ya se ha seleccionado la frecuencia de adquisicion
jjmedina	0:a5367bd4e404	169
jjmedina	0:a5367bd4e404	170
jjmedina	0:a5367bd4e404	171	//////////////////////////////////////////
jjmedina	0:a5367bd4e404	172	/// ALMACENAMIENTO DE DATOS CON BUFFER ///
jjmedina	0:a5367bd4e404	173	//////////////////////////////////////////
jjmedina	0:a5367bd4e404	174
jjmedina	0:a5367bd4e404	175	void LeeAcelerometroBuffer();
jjmedina	0:a5367bd4e404	176	void GuardaDatosBuffer();
jjmedina	0:a5367bd4e404	177
jjmedina	0:a5367bd4e404	178	int readings[3] = {0, 0, 0}; //Lectura aceleraciones sensor digital
jjmedina	0:a5367bd4e404	179	float aX, aY, aZ; //Medidas de aceleracion
jjmedina	0:a5367bd4e404	180	int16_t aaX, aaY, aaZ; //Medidas de aceleracion y tiempo
jjmedina	0:a5367bd4e404	181
jjmedina	0:a5367bd4e404	182	float aXr, aYr, aZr; //Tiempo y medidas de aceleracion leidos del buffer
jjmedina	0:a5367bd4e404	183	unsigned int contDatos = 0; //Contador de datos
jjmedina	0:a5367bd4e404	184
jjmedina	0:a5367bd4e404	185	int sizeBuff = 500; //Tamaño de los buffer que guardan los datos
jjmedina	0:a5367bd4e404	186
jjmedina	0:a5367bd4e404	187	Ticker tickerLectura;
jjmedina	0:a5367bd4e404	188
jjmedina	0:a5367bd4e404	189	Buffer buff_aX(sizeBuff);
jjmedina	0:a5367bd4e404	190	Buffer buff_aY(sizeBuff);
jjmedina	0:a5367bd4e404	191	Buffer buff_aZ(sizeBuff);
jjmedina	0:a5367bd4e404	192
jjmedina	0:a5367bd4e404	193	FILE * fp;
jjmedina	0:a5367bd4e404	194	FILE * fp2;
jjmedina	0:a5367bd4e404	195
jjmedina	0:a5367bd4e404	196	unsigned int ContadorEnsayos = 0;
jjmedina	0:a5367bd4e404	197	unsigned int ContadorDatos = 0;
jjmedina	0:a5367bd4e404	198	unsigned int ContadorMedidas = 0;
jjmedina	0:a5367bd4e404	199
jjmedina	0:a5367bd4e404	200	void ParamDependFrecAdq(void);
jjmedina	0:a5367bd4e404	201
jjmedina	0:a5367bd4e404	202	int main() {
JuanManuelAmador	2:cc4a43d806e2	203	pc.baud(115200);
jjmedina	0:a5367bd4e404	204
jjmedina	0:a5367bd4e404	205	timer.start();
jjmedina	0:a5367bd4e404	206
jjmedina	0:a5367bd4e404	207	bool datosPC_green = false; //true = envia datos a PC, false = Guarda datos en tarjeta SD
jjmedina	0:a5367bd4e404	208	bool ensayo_red = false; //true = Comienza la medida, false = termina la medida
jjmedina	0:a5367bd4e404	209
jjmedina	0:a5367bd4e404	210	char NoViaje[2];
jjmedina	0:a5367bd4e404	211
jjmedina	0:a5367bd4e404	212	//////////////////////////////////////////////
jjmedina	0:a5367bd4e404	213	/// ESTABLECE LOS OFFSETS DEL ACELEROMETRO ///
jjmedina	0:a5367bd4e404	214	//////////////////////////////////////////////
jjmedina	0:a5367bd4e404	215
jjmedina	0:a5367bd4e404	216	char offsets[6];
jjmedina	0:a5367bd4e404	217
jjmedina	0:a5367bd4e404	218	offsets[0]= 0xAE; //x_msb
jjmedina	0:a5367bd4e404	219	offsets[1]= 0x9E; //y_msb
jjmedina	0:a5367bd4e404	220	offsets[2]= 0x95; //z_msb
jjmedina	0:a5367bd4e404	221
jjmedina	0:a5367bd4e404	222	offsets[3]= 0x0C; //x_lsb
jjmedina	0:a5367bd4e404	223	offsets[4]= 0x04; //y_lsb
jjmedina	0:a5367bd4e404	224	offsets[5]= 0x0C; //z_lsb
jjmedina	0:a5367bd4e404	225
jjmedina	0:a5367bd4e404	226
jjmedina	0:a5367bd4e404	227	//bma180.AcelerometroLeeOffsets(readings);
jjmedina	0:a5367bd4e404	228	//pc.printf("%d %d %d\n", readings[0], readings[1], readings[2]);
jjmedina	0:a5367bd4e404	229	//bma180.AcelerometroEscribeOffsets(offsets);
jjmedina	0:a5367bd4e404	230	//bma180.AcelerometroLeeOffsets(readings);
jjmedina	0:a5367bd4e404	231	//pc.printf("%d %d %d\n", readings[0], readings[1], readings[2]);
jjmedina	0:a5367bd4e404	232
jjmedina	0:a5367bd4e404	233	/////////////////////////////
jjmedina	0:a5367bd4e404	234	/// APAGA EL ACELEROMETRO ///
jjmedina	0:a5367bd4e404	235	/////////////////////////////
jjmedina	0:a5367bd4e404	236
jjmedina	0:a5367bd4e404	237	bma180.AcelerometroSleep();
jjmedina	0:a5367bd4e404	238
jjmedina	0:a5367bd4e404	239	/////////////////////////////////////
jjmedina	0:a5367bd4e404	240	/// SELECCION DE IDIOMA DE LA GUI ///
jjmedina	0:a5367bd4e404	241	/////////////////////////////////////
jjmedina	0:a5367bd4e404	242
jjmedina	0:a5367bd4e404	243	display.Clear(); display.SetXY(0,0); display.printf("English: yellow"); display.SetXY(0,1); display.printf("Castellano: rojo");
jjmedina	0:a5367bd4e404	244
jjmedina	0:a5367bd4e404	245	while(SeleccionIdioma == 0)
jjmedina	0:a5367bd4e404	246	{
jjmedina	0:a5367bd4e404	247	if(red_PUSH.read() == 1)
jjmedina	0:a5367bd4e404	248	{
jjmedina	0:a5367bd4e404	249	TRADUCE = 0;//traduce gui al español
jjmedina	0:a5367bd4e404	250	SeleccionIdioma = 1;
jjmedina	0:a5367bd4e404	251	wait(0.5);
jjmedina	0:a5367bd4e404	252	}
jjmedina	0:a5367bd4e404	253	if(green_PUSH.read() == 1)
jjmedina	0:a5367bd4e404	254	{
jjmedina	0:a5367bd4e404	255	TRADUCE = 1;//traduce gui al ingles
jjmedina	0:a5367bd4e404	256	SeleccionIdioma = 1;
jjmedina	0:a5367bd4e404	257	wait(0.5);
jjmedina	0:a5367bd4e404	258	}
jjmedina	0:a5367bd4e404	259	}
jjmedina	0:a5367bd4e404	260
jjmedina	0:a5367bd4e404	261	/////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	262	/// SELECCION DE LA FRECUENCIA DE ADQUISICION ///
jjmedina	0:a5367bd4e404	263	/////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	264
jjmedina	0:a5367bd4e404	265	if(TRADUCE == 0)
jjmedina	0:a5367bd4e404	266	{
jjmedina	0:a5367bd4e404	267	display.Clear(); display.SetXY(0,0); display.printf("320 Hz: amarillo"); display.SetXY(0,1); display.printf("1000 Hz: rojo");
jjmedina	0:a5367bd4e404	268	}
jjmedina	0:a5367bd4e404	269	if(TRADUCE == 1)
jjmedina	0:a5367bd4e404	270	{
jjmedina	0:a5367bd4e404	271	display.Clear(); display.SetXY(0,0); display.printf("320 Hz: yellow"); display.SetXY(0,1); display.printf("1000 Hz: red");
jjmedina	0:a5367bd4e404	272	}
jjmedina	0:a5367bd4e404	273
jjmedina	0:a5367bd4e404	274	while(SeleccionFrecAdq == 0)
jjmedina	0:a5367bd4e404	275	{
jjmedina	0:a5367bd4e404	276	if(red_PUSH.read() == 1)
jjmedina	0:a5367bd4e404	277	{
jjmedina	0:a5367bd4e404	278	FrecAdq = 1000;
jjmedina	0:a5367bd4e404	279	SeleccionFrecAdq = 1;
jjmedina	0:a5367bd4e404	280	wait(0.5);
jjmedina	0:a5367bd4e404	281	}
jjmedina	0:a5367bd4e404	282	if(green_PUSH.read() == 1)
jjmedina	0:a5367bd4e404	283	{
jjmedina	0:a5367bd4e404	284	FrecAdq = 320;
jjmedina	0:a5367bd4e404	285	SeleccionFrecAdq = 1;
jjmedina	0:a5367bd4e404	286	wait(0.5);
jjmedina	0:a5367bd4e404	287	}
jjmedina	0:a5367bd4e404	288	}
jjmedina	0:a5367bd4e404	289
jjmedina	0:a5367bd4e404	290	ParamDependFrecAdq(); //Establece el valor de los parámetros que depeneden de la frecuencia de adquisicion
jjmedina	0:a5367bd4e404	291
jjmedina	0:a5367bd4e404	292	////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	293	/// AVISA MODO FUNCIONAMIENTO: PC O AUTONOMO ///
jjmedina	0:a5367bd4e404	294	////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	295
jjmedina	0:a5367bd4e404	296	if(datosPC_green == true)
jjmedina	0:a5367bd4e404	297	{
jjmedina	0:a5367bd4e404	298	green_LED = 1;
jjmedina	0:a5367bd4e404	299	if(TRADUCE == 0)
jjmedina	0:a5367bd4e404	300	{
jjmedina	0:a5367bd4e404	301	display.Clear(); display.SetXY(0,0); display.printf("Aparato en"); display.SetXY(0,1); display.printf("modo PC.");
jjmedina	0:a5367bd4e404	302	wait(1.5);
jjmedina	0:a5367bd4e404	303	display.Clear(); display.SetXY(0,0); display.printf("Para cambio a"); display.SetXY(0,1); display.printf("modo autonomo...");
jjmedina	0:a5367bd4e404	304	wait(1.5);
jjmedina	0:a5367bd4e404	305	display.Clear(); display.SetXY(0,0); display.printf("...pulse boton"); display.SetXY(0,1); display.printf("amarillo.");
jjmedina	0:a5367bd4e404	306	wait(1.5);
jjmedina	0:a5367bd4e404	307	display.Clear();
jjmedina	0:a5367bd4e404	308	}
jjmedina	0:a5367bd4e404	309	else
jjmedina	0:a5367bd4e404	310	{
jjmedina	0:a5367bd4e404	311	display.Clear(); display.SetXY(0,0); display.printf("Set in PC mode.");
jjmedina	0:a5367bd4e404	312	wait(1.5);
jjmedina	0:a5367bd4e404	313	display.Clear(); display.SetXY(0,0); display.printf("To change to"); display.SetXY(0,1); display.printf("autonomous mode");
jjmedina	0:a5367bd4e404	314	wait(1.5);
jjmedina	0:a5367bd4e404	315	display.Clear(); display.SetXY(0,0); display.printf("...press"); display.SetXY(0,1); display.printf("yellow button.");
jjmedina	0:a5367bd4e404	316	wait(1.5);
jjmedina	0:a5367bd4e404	317	display.Clear();
jjmedina	0:a5367bd4e404	318	}
jjmedina	0:a5367bd4e404	319	}
jjmedina	0:a5367bd4e404	320	else
jjmedina	0:a5367bd4e404	321	{
jjmedina	0:a5367bd4e404	322	green_LED = 0;
jjmedina	0:a5367bd4e404	323
jjmedina	0:a5367bd4e404	324	if(TRADUCE == 0)
jjmedina	0:a5367bd4e404	325	{
jjmedina	0:a5367bd4e404	326	display.Clear(); display.SetXY(0,0); display.printf("Aparato en"); display.SetXY(0,1); display.printf("modo autonomo.");
jjmedina	0:a5367bd4e404	327	wait(1.5);
jjmedina	0:a5367bd4e404	328	display.Clear(); display.SetXY(0,0); display.printf("Para cambio a"); display.SetXY(0,1); display.printf("modo PC...");
jjmedina	0:a5367bd4e404	329	wait(1.5);
jjmedina	0:a5367bd4e404	330	display.Clear(); display.SetXY(0,0); display.printf("...pulse boton"); display.SetXY(0,1); display.printf("amarillo.");
jjmedina	0:a5367bd4e404	331	wait(1.5);
jjmedina	0:a5367bd4e404	332	display.Clear();
jjmedina	0:a5367bd4e404	333	}
jjmedina	0:a5367bd4e404	334	else
jjmedina	0:a5367bd4e404	335	{
jjmedina	0:a5367bd4e404	336	display.Clear(); display.SetXY(0,0); display.printf("Set in "); display.SetXY(0,1); display.printf("autonomous mode.");
jjmedina	0:a5367bd4e404	337	wait(1.5);
jjmedina	0:a5367bd4e404	338	display.Clear(); display.SetXY(0,0); display.printf("To change to"); display.SetXY(0,1); display.printf("PC mode...");
jjmedina	0:a5367bd4e404	339	wait(1.5);
jjmedina	0:a5367bd4e404	340	display.Clear(); display.SetXY(0,0); display.printf("...press"); display.SetXY(0,1); display.printf("yellow button.");
jjmedina	0:a5367bd4e404	341	wait(1.5);
jjmedina	0:a5367bd4e404	342	display.Clear();
jjmedina	0:a5367bd4e404	343	}
jjmedina	0:a5367bd4e404	344	}
jjmedina	0:a5367bd4e404	345
jjmedina	0:a5367bd4e404	346
jjmedina	0:a5367bd4e404	347
jjmedina	0:a5367bd4e404	348	while((SeleccionIdioma == 1)&&(SeleccionFrecAdq == 1))
jjmedina	0:a5367bd4e404	349	{
jjmedina	0:a5367bd4e404	350
jjmedina	0:a5367bd4e404	351	////////////////////////////////////////
jjmedina	0:a5367bd4e404	352	/// INVITA A COMENZAR LA ADQUISICION ///
jjmedina	0:a5367bd4e404	353	////////////////////////////////////////
jjmedina	0:a5367bd4e404	354
jjmedina	0:a5367bd4e404	355	if(ensayo_red == false)
jjmedina	0:a5367bd4e404	356	{
jjmedina	0:a5367bd4e404	357	if(TRADUCE == 0)
jjmedina	0:a5367bd4e404	358	{
jjmedina	0:a5367bd4e404	359	display.Clear();
jjmedina	0:a5367bd4e404	360	display.SetXY(0,0); display.printf("Pulse rojo");
jjmedina	0:a5367bd4e404	361	display.SetXY(0,1); display.printf("para empezar");
jjmedina	0:a5367bd4e404	362	wait(0.1);
jjmedina	0:a5367bd4e404	363	}
jjmedina	0:a5367bd4e404	364	if(TRADUCE == 1)
jjmedina	0:a5367bd4e404	365	{
jjmedina	0:a5367bd4e404	366	display.Clear();
jjmedina	0:a5367bd4e404	367	display.SetXY(0,0); display.printf("Push red");
jjmedina	0:a5367bd4e404	368	display.SetXY(0,1); display.printf("to start");
jjmedina	0:a5367bd4e404	369	wait(0.1);
jjmedina	0:a5367bd4e404	370	}
jjmedina	0:a5367bd4e404	371	}
jjmedina	0:a5367bd4e404	372	///////////////////////////////
jjmedina	0:a5367bd4e404	373	// COMIENZO Y FIN DE MEDIDDA //
jjmedina	0:a5367bd4e404	374	///////////////////////////////
jjmedina	0:a5367bd4e404	375
jjmedina	0:a5367bd4e404	376	if(red_PUSH.read() == 1) //si pulsa el rojo (o empieza o acaba medidas.)
jjmedina	0:a5367bd4e404	377	{
jjmedina	0:a5367bd4e404	378	if(ensayo_red == false)//antes de pulsar estaba apagado, por tanto empieza medidas.
jjmedina	0:a5367bd4e404	379	{
jjmedina	0:a5367bd4e404	380	ensayo_red = true;//lo enciende
jjmedina	0:a5367bd4e404	381
jjmedina	0:a5367bd4e404	382	////////////////////////////////
jjmedina	0:a5367bd4e404	383	/// ENCIENDE EL ACELEROMETRO ///
jjmedina	0:a5367bd4e404	384	////////////////////////////////
jjmedina	0:a5367bd4e404	385
jjmedina	0:a5367bd4e404	386	bma180.AcelerometroWakeUp();
jjmedina	0:a5367bd4e404	387
jjmedina	0:a5367bd4e404	388	if(datosPC_green == false)//Para empezar a medir y almecenar en USB. prepara ficheros de almacenamiento USB etc. si LED verde está apagado (modo autonomo)
jjmedina	0:a5367bd4e404	389	{
jjmedina	0:a5367bd4e404	390	// LEE DE FICHERO EL ORDEN DEL ENSAYO ANTERIOR Y VUELVE A GUARDAR EL ORDEN INCREMENTADO EN 1
jjmedina	0:a5367bd4e404	391	fp2 = fopen("/mbedFlash/OrdEns.txt","r");
jjmedina	0:a5367bd4e404	392	if(fp2 == NULL)
jjmedina	0:a5367bd4e404	393	{
jjmedina	0:a5367bd4e404	394	//SI NO EXISTE EL ARCHIVO CON EL ORDEN DEL ENSAYO SE CREA Y SE PONE UN 0
jjmedina	0:a5367bd4e404	395	pc.printf("Could not open file OrdEns.txt for read\n");
jjmedina	0:a5367bd4e404	396	fp2 = fopen("/mbedFlash/OrdEns.txt","w");
jjmedina	0:a5367bd4e404	397	if(fp2 == NULL) { pc.printf("Could not open file OrdEns.txt for write\n"); }
jjmedina	0:a5367bd4e404	398	fprintf(fp2,"0");
jjmedina	0:a5367bd4e404	399	fclose(fp2);
jjmedina	0:a5367bd4e404	400	}
jjmedina	0:a5367bd4e404	401	fscanf(fp2,"%d", &ContadorEnsayos);
jjmedina	0:a5367bd4e404	402	ContadorEnsayos++;
jjmedina	0:a5367bd4e404	403
jjmedina	0:a5367bd4e404	404	if(TRADUCE==0)
jjmedina	0:a5367bd4e404	405	{
jjmedina	0:a5367bd4e404	406	display.Clear(); display.SetXY(0,0); display.printf("Comienza medida"); display.SetXY(0,1); display.printf("Viaje %d ...", ContadorEnsayos);
jjmedina	0:a5367bd4e404	407	}
jjmedina	0:a5367bd4e404	408	else
jjmedina	0:a5367bd4e404	409	{
jjmedina	0:a5367bd4e404	410	display.Clear(); display.SetXY(0,0); display.printf("Measuring ride"); display.SetXY(0,1); display.printf("number %d ...", ContadorEnsayos);
jjmedina	0:a5367bd4e404	411	}
jjmedina	0:a5367bd4e404	412
jjmedina	0:a5367bd4e404	413
jjmedina	0:a5367bd4e404	414	if(ContadorEnsayos == 100) { ContadorEnsayos = 0; } //EL NUMERO MAXIMO DE ENSAYOS ES 100. SI SE SUPERA SE PONE EL CONTADOR A 0
jjmedina	0:a5367bd4e404	415	pc.printf("Ensayo: %d\n",ContadorEnsayos);
jjmedina	0:a5367bd4e404	416	fclose(fp2);
jjmedina	0:a5367bd4e404	417	fp2 = fopen("/mbedFlash/OrdEns.txt","w");
jjmedina	0:a5367bd4e404	418	if(fp2 == NULL) { pc.printf("Could not open file OrdEns.txt for write\n"); }
jjmedina	0:a5367bd4e404	419	fprintf(fp2,"%d", ContadorEnsayos);
jjmedina	0:a5367bd4e404	420	fclose(fp2);
jjmedina	0:a5367bd4e404	421
jjmedina	0:a5367bd4e404	422	ContadorDatos = 0;
jjmedina	0:a5367bd4e404	423	ContadorMedidas = 0;
jjmedina	0:a5367bd4e404	424	//Crea y abre un nuevo archivo para guardar aceleraciones
jjmedina	0:a5367bd4e404	425	char resultsFile[50];
jjmedina	0:a5367bd4e404	426
jjmedina	0:a5367bd4e404	427	strcpy(resultsFile,"/local/raw_V_");
jjmedina	0:a5367bd4e404	428
jjmedina	0:a5367bd4e404	429	int n = sprintf (NoViaje, "%d", ContadorEnsayos);
jjmedina	0:a5367bd4e404	430	strcat(resultsFile,NoViaje);
jjmedina	0:a5367bd4e404	431	strcat(resultsFile,".txt");
jjmedina	0:a5367bd4e404	432
jjmedina	0:a5367bd4e404	433	msc.disk_initialize();
jjmedina	0:a5367bd4e404	434
jjmedina	0:a5367bd4e404	435	fp = fopen(resultsFile,"w");
jjmedina	0:a5367bd4e404	436
jjmedina	0:a5367bd4e404	437	if(fp == NULL)
jjmedina	0:a5367bd4e404	438	{
jjmedina	0:a5367bd4e404	439	if(TRADUCE==0)
jjmedina	0:a5367bd4e404	440	{
jjmedina	0:a5367bd4e404	441	display.Clear();display.SetXY(0,0); display.printf("USB desconectado");display.SetXY(0,1); display.printf("o lleno");
jjmedina	0:a5367bd4e404	442	}
jjmedina	0:a5367bd4e404	443	else
jjmedina	0:a5367bd4e404	444	{
jjmedina	0:a5367bd4e404	445	display.Clear();display.SetXY(0,0); display.printf("USB disconnected");display.SetXY(0,1); display.printf("or full");
jjmedina	0:a5367bd4e404	446	}
jjmedina	0:a5367bd4e404	447	///////////////////////////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	448	/// LO DE ABAJO ES PARA QUE NO SE QUEDE COLGADO SI HEMOS OLVIDADO CONECTAR EL PEN-DRIVE ///
jjmedina	0:a5367bd4e404	449	///////////////////////////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	450	ensayo_red = false;
jjmedina	0:a5367bd4e404	451	ContadorEnsayos--;
jjmedina	0:a5367bd4e404	452	fp2 = fopen("/mbedFlash/OrdEns.txt","w");
jjmedina	0:a5367bd4e404	453	if(fp2 == NULL) { pc.printf("Could not open file OrdEns.txt for write\n"); }
jjmedina	0:a5367bd4e404	454	fprintf(fp2,"%d", ContadorEnsayos);
jjmedina	0:a5367bd4e404	455	fclose(fp2);
jjmedina	0:a5367bd4e404	456	}
jjmedina	0:a5367bd4e404	457
jjmedina	0:a5367bd4e404	458	/////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	459	/// OJO: COMIENZA EL TICKER DE LECTURA DE DATOS DEL ACELERÓMETRO ///
jjmedina	0:a5367bd4e404	460	/////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	461	if(ensayo_red == true)
jjmedina	0:a5367bd4e404	462	{
jjmedina	0:a5367bd4e404	463	tickerLectura.attach_us(&LeeAcelerometroBuffer, PasoTiempo);
jjmedina	0:a5367bd4e404	464	}
jjmedina	0:a5367bd4e404	465	}
jjmedina	0:a5367bd4e404	466
jjmedina	0:a5367bd4e404	467	wait(0.5);
jjmedina	0:a5367bd4e404	468	}
jjmedina	0:a5367bd4e404	469	else //antes de pulsar estaba encendido
jjmedina	0:a5367bd4e404	470	{
jjmedina	0:a5367bd4e404	471	ensayo_red = false; // lo apaga, es decir acaba medidas y procesa datos si en modo autonomo
jjmedina	0:a5367bd4e404	472	if(ensayo_red == true) { red_LED = 1;} else { red_LED = 0;}//enciende o apaga el LED rojo
jjmedina	0:a5367bd4e404	473
jjmedina	0:a5367bd4e404	474	pc.printf("-25717505\n"); //Señal para vmRideanalizer
jjmedina	0:a5367bd4e404	475
jjmedina	0:a5367bd4e404	476	/////////////////////////////
jjmedina	0:a5367bd4e404	477	/// APAGA EL ACELEROMETRO ///
jjmedina	0:a5367bd4e404	478	/////////////////////////////
jjmedina	0:a5367bd4e404	479
jjmedina	0:a5367bd4e404	480	bma180.AcelerometroSleep();
jjmedina	0:a5367bd4e404	481
jjmedina	0:a5367bd4e404	482	if(datosPC_green == false)//procesa datos si en modo autonomo. Cuando entra aquí, estan LEDs apagados + ensayo_red = false + datosPC_green == false
jjmedina	0:a5367bd4e404	483	{
jjmedina	0:a5367bd4e404	484	///////////////////////////////////////////
jjmedina	0:a5367bd4e404	485	// POSTPROCESO DE DATOS EN MODO AUTONOMO //Primera tanda
jjmedina	0:a5367bd4e404	486	///////////////////////////////////////////
jjmedina	0:a5367bd4e404	487
jjmedina	0:a5367bd4e404	488	/////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	489	/// OJO: FINALIZA EL TICKER DE LECTURA DE DATOS DEL ACELERÓMETRO ///
jjmedina	0:a5367bd4e404	490	/////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	491
jjmedina	0:a5367bd4e404	492	tickerLectura.detach();
jjmedina	0:a5367bd4e404	493
jjmedina	0:a5367bd4e404	494	fclose(fp);//cierra fichero con datos en el USB
jjmedina	0:a5367bd4e404	495
jjmedina	0:a5367bd4e404	496	//if(TRADUCE==0)
jjmedina	0:a5367bd4e404	497	//{
jjmedina	0:a5367bd4e404	498	// display.Clear();
jjmedina	0:a5367bd4e404	499	// display.SetXY(0,0); display.printf("Interpola datos");
jjmedina	0:a5367bd4e404	500	//}else
jjmedina	0:a5367bd4e404	501	//{
jjmedina	0:a5367bd4e404	502	// display.Clear();
jjmedina	0:a5367bd4e404	503	// display.SetXY(0,0); display.printf("Interpolating data");
jjmedina	0:a5367bd4e404	504	// display.SetXY(0,1); display.printf("data");
jjmedina	0:a5367bd4e404	505	//}
jjmedina	0:a5367bd4e404	506	//InterpolaMedidasPerdidas(ContadorEnsayos, ContadorDatos);
jjmedina	0:a5367bd4e404	507
jjmedina	0:a5367bd4e404	508	if(TRADUCE==0)
jjmedina	0:a5367bd4e404	509	{
jjmedina	0:a5367bd4e404	510	display.Clear(); display.SetXY(0,0); display.printf("Rojo: posprocesa"); display.SetXY(0,1); display.printf("Amarillo: fin");
jjmedina	0:a5367bd4e404	511	}else
jjmedina	0:a5367bd4e404	512	{
jjmedina	0:a5367bd4e404	513	display.Clear(); display.SetXY(0,0); display.printf("Red: postprocess"); display.SetXY(0,1); display.printf("Yellow: finalize");
jjmedina	0:a5367bd4e404	514	}
jjmedina	0:a5367bd4e404	515
jjmedina	0:a5367bd4e404	516	wait(0.5);
jjmedina	0:a5367bd4e404	517
jjmedina	0:a5367bd4e404	518	while(1)
jjmedina	0:a5367bd4e404	519	{
jjmedina	0:a5367bd4e404	520	///////////////////////////////////////////
jjmedina	0:a5367bd4e404	521	// POSTPROCESO DE DATOS EN MODO AUTONOMO //Primera tanda
jjmedina	0:a5367bd4e404	522	///////////////////////////////////////////
jjmedina	0:a5367bd4e404	523	//loop infinito hasta que pulsa el rojo otra vez para ver mas resultados o el verde para terminar y grabar resultados en fichero
jjmedina	0:a5367bd4e404	524	if(red_PUSH.read() == 1) //si pulsa el rojo
jjmedina	0:a5367bd4e404	525	{
jjmedina	0:a5367bd4e404	526	//ponemos variables globales a zero
jjmedina	0:a5367bd4e404	527	//aceleraciones medias
jjmedina	0:a5367bd4e404	528	aZm0 = 0.0;
jjmedina	0:a5367bd4e404	529	aYm0 = 0.0;
jjmedina	0:a5367bd4e404	530	aXm0 = 0.0;
jjmedina	0:a5367bd4e404	531	//veloc truco Dani
jjmedina	0:a5367bd4e404	532	vZmFin = 0.0;
jjmedina	0:a5367bd4e404	533	vNom = 0.0;
jjmedina	0:a5367bd4e404	534	//maximos ISO
jjmedina	0:a5367bd4e404	535	amax = 0.0;
jjmedina	0:a5367bd4e404	536	dmax = 0.0;
jjmedina	0:a5367bd4e404	537	Vmax = 0.0;
jjmedina	0:a5367bd4e404	538	jerkmax = 0.0;
jjmedina	0:a5367bd4e404	539	max_peaktopeak_car_ = 0.0;
jjmedina	0:a5367bd4e404	540	max_peaktopeak_ncar_ = 0.0;
jjmedina	0:a5367bd4e404	541	max_peaktopeak_X_ = 0.0;
jjmedina	0:a5367bd4e404	542	max_peaktopeak_Y_ = 0.0;
jjmedina	0:a5367bd4e404	543	Vmax1 = 0.0;
jjmedina	0:a5367bd4e404	544	Vmax2 = 0.0;
jjmedina	0:a5367bd4e404	545
jjmedina	0:a5367bd4e404	546	if(TRADUCE==0)
jjmedina	0:a5367bd4e404	547	{
jjmedina	0:a5367bd4e404	548	display.Clear(); display.SetXY(0,0); display.printf("Comienza el"); display.SetXY(0,1); display.printf("postproceso ...");
jjmedina	0:a5367bd4e404	549	}
jjmedina	0:a5367bd4e404	550	else
jjmedina	0:a5367bd4e404	551	{
jjmedina	0:a5367bd4e404	552	display.Clear(); display.SetXY(0,0); display.printf("Starting"); display.SetXY(0,1); display.printf("postprocess ...");
jjmedina	0:a5367bd4e404	553	}
jjmedina	0:a5367bd4e404	554
jjmedina	0:a5367bd4e404	555	//InterpolaMedidasPerdidas(ContadorEnsayos, ContadorDatos);
jjmedina	0:a5367bd4e404	556
jjmedina	0:a5367bd4e404	557	FiltradoPeriodosIntegracion(ContadorEnsayos, ContadorDatos);
jjmedina	0:a5367bd4e404	558
jjmedina	0:a5367bd4e404	559	//display.Clear(); display.SetXY(0,0); display.printf("Interpolacion,"); display.SetXY(0,1); display.printf("filtro, integracion");
jjmedina	0:a5367bd4e404	560
jjmedina	0:a5367bd4e404	561	DatosConfort(ContadorEnsayos, ContadorDatos, PasoTiempo);
jjmedina	0:a5367bd4e404	562
jjmedina	0:a5367bd4e404	563	//display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("Vel. nom. y jerk");
jjmedina	0:a5367bd4e404	564
jjmedina	0:a5367bd4e404	565	calcula_n05n95_Boveloc_jerkmax(ContadorEnsayos, ContadorDatos, PasoTiempo);
jjmedina	0:a5367bd4e404	566
jjmedina	0:a5367bd4e404	567	//ha terminado de calcular amax, dmax, Vmax y jerkmax
jjmedina	0:a5367bd4e404	568
jjmedina	0:a5367bd4e404	569	//display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("A95s");
jjmedina	0:a5367bd4e404	570
jjmedina	0:a5367bd4e404	571	calcula_A95_V_acel_decel(ContadorEnsayos, ContadorDatos, PasoTiempo);
jjmedina	0:a5367bd4e404	572
jjmedina	0:a5367bd4e404	573	//ha terminado de calcular A95 de a, d y V. Muestra por pantalla
jjmedina	0:a5367bd4e404	574
jjmedina	0:a5367bd4e404	575	DisplayResultados1();
jjmedina	0:a5367bd4e404	576
jjmedina	0:a5367bd4e404	577
jjmedina	0:a5367bd4e404	578	if(FrecAdq == 320)
jjmedina	0:a5367bd4e404	579	{
jjmedina	0:a5367bd4e404	580	if(TRADUCE==0)
jjmedina	0:a5367bd4e404	581	{
jjmedina	0:a5367bd4e404	582	display.Clear(); display.SetXY(0,0); display.printf("Rojo continua"); display.SetXY(0,1); display.printf("Amarillo fin");
jjmedina	0:a5367bd4e404	583	}else
jjmedina	0:a5367bd4e404	584	{
jjmedina	0:a5367bd4e404	585	display.Clear(); display.SetXY(0,0); display.printf("Red continue"); display.SetXY(0,1); display.printf("Yellow finalize");
jjmedina	0:a5367bd4e404	586	}
jjmedina	0:a5367bd4e404	587	}
jjmedina	0:a5367bd4e404	588	break;//ha calculado resultados restantes y finaliza
jjmedina	0:a5367bd4e404	589	}else if(green_PUSH.read() == 1)
jjmedina	0:a5367bd4e404	590	{
jjmedina	0:a5367bd4e404	591	break;
jjmedina	0:a5367bd4e404	592	}
jjmedina	0:a5367bd4e404	593	}//primera tanda resultados
jjmedina	0:a5367bd4e404	594
jjmedina	0:a5367bd4e404	595
jjmedina	0:a5367bd4e404	596
jjmedina	0:a5367bd4e404	597	while(FrecAdq == 320)
jjmedina	0:a5367bd4e404	598	{
jjmedina	0:a5367bd4e404	599	///////////////////////////////////////////
jjmedina	0:a5367bd4e404	600	// POSTPROCESO DE DATOS EN MODO AUTONOMO //Segunda tanda
jjmedina	0:a5367bd4e404	601	///////////////////////////////////////////
jjmedina	0:a5367bd4e404	602
jjmedina	0:a5367bd4e404	603	//loop infinito hasta que pulsa el rojo otra vez para ver mas resultados o el verde para terminar y grabar resultados en fichero
jjmedina	0:a5367bd4e404	604
jjmedina	0:a5367bd4e404	605	if(red_PUSH.read() == 1) //si pulsa el rojo
jjmedina	0:a5367bd4e404	606	{
jjmedina	0:a5367bd4e404	607	if(TRADUCE==0)
jjmedina	0:a5367bd4e404	608	{
jjmedina	0:a5367bd4e404	609	display.Clear(); display.SetXY(0,0); display.printf("Cont. Posproceso"); display.SetXY(0,1); display.printf("vibracion Z,X,Y");
jjmedina	0:a5367bd4e404	610	}else
jjmedina	0:a5367bd4e404	611	{
jjmedina	0:a5367bd4e404	612	display.Clear(); display.SetXY(0,0); display.printf("Con. Postprocess"); display.SetXY(0,1); display.printf("vibration Z,X,Y");
jjmedina	0:a5367bd4e404	613	}
jjmedina	0:a5367bd4e404	614	//empieza cálculos de ZXYvibweighted
jjmedina	0:a5367bd4e404	615	vibraReadFilter(ContadorEnsayos, ContadorDatos);
jjmedina	0:a5367bd4e404	616
jjmedina	0:a5367bd4e404	617	//display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("step2");
jjmedina	0:a5367bd4e404	618
jjmedina	0:a5367bd4e404	619	calculaStep2( ContadorEnsayos, ContadorDatos);
jjmedina	0:a5367bd4e404	620
jjmedina	0:a5367bd4e404	621	//display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("car-ncar");
jjmedina	0:a5367bd4e404	622
jjmedina	0:a5367bd4e404	623	calculaCarNCar( ContadorEnsayos, ContadorDatos, PasoTiempo ); //pc.printf("Termina 'calculaCarNCar'\n");
jjmedina	0:a5367bd4e404	624
jjmedina	0:a5367bd4e404	625	//display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("maxZXY");
jjmedina	0:a5367bd4e404	626
jjmedina	0:a5367bd4e404	627	calculaZXYvibweighted( ContadorEnsayos, ContadorDatos );
jjmedina	0:a5367bd4e404	628
jjmedina	0:a5367bd4e404	629	//display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("Display");
jjmedina	0:a5367bd4e404	630
jjmedina	0:a5367bd4e404	631	DisplayResultados2();
jjmedina	0:a5367bd4e404	632
jjmedina	0:a5367bd4e404	633	break;//ha calculado resultados restantes y finaliza
jjmedina	0:a5367bd4e404	634	}else if(green_PUSH.read() == 1)
jjmedina	0:a5367bd4e404	635	{
jjmedina	0:a5367bd4e404	636	break;
jjmedina	0:a5367bd4e404	637	}
jjmedina	0:a5367bd4e404	638	}
jjmedina	0:a5367bd4e404	639
jjmedina	0:a5367bd4e404	640	//segunda tanda resultados
jjmedina	0:a5367bd4e404	641	//copia resultados en un fichero y finaliza
jjmedina	0:a5367bd4e404	642	if(TRADUCE==0)
jjmedina	0:a5367bd4e404	643	{
jjmedina	0:a5367bd4e404	644	display.Clear(); display.SetXY(0,0); display.printf("Guarda resultados"); display.SetXY(0,1); display.printf("en fichero");
jjmedina	0:a5367bd4e404	645	}else
jjmedina	0:a5367bd4e404	646	{
jjmedina	0:a5367bd4e404	647	display.Clear(); display.SetXY(0,0); display.printf("Save results"); display.SetXY(0,1); display.printf("in file");
jjmedina	0:a5367bd4e404	648	}
jjmedina	0:a5367bd4e404	649
jjmedina	0:a5367bd4e404	650	FileResultados( ContadorEnsayos );
jjmedina	0:a5367bd4e404	651
jjmedina	0:a5367bd4e404	652	wait(0.5);
jjmedina	0:a5367bd4e404	653
jjmedina	0:a5367bd4e404	654	if(TRADUCE==0)
jjmedina	0:a5367bd4e404	655	{
jjmedina	0:a5367bd4e404	656	display.Clear(); display.SetXY(0,0); display.printf("Postproceso"); display.SetXY(0,1); display.printf("fin");
jjmedina	0:a5367bd4e404	657	}else
jjmedina	0:a5367bd4e404	658	{
jjmedina	0:a5367bd4e404	659	display.Clear(); display.SetXY(0,0); display.printf("Postprocess"); display.SetXY(0,1); display.printf("end");
jjmedina	0:a5367bd4e404	660	}
jjmedina	0:a5367bd4e404	661	}
jjmedina	0:a5367bd4e404	662	wait(0.5);
jjmedina	0:a5367bd4e404	663	display.Clear();
jjmedina	0:a5367bd4e404	664	}
jjmedina	0:a5367bd4e404	665	}
jjmedina	0:a5367bd4e404	666
jjmedina	0:a5367bd4e404	667	if(ensayo_red == true) { red_LED = 1;} else { red_LED = 0;}//enciende o apaga el LED rojo
jjmedina	0:a5367bd4e404	668
jjmedina	0:a5367bd4e404	669	/////////////////////////////
jjmedina	0:a5367bd4e404	670	///MODOS DE FUNCIONAMIENTO://
jjmedina	0:a5367bd4e404	671	// AUTONOMO O PC ////////////
jjmedina	0:a5367bd4e404	672	/////////////////////////////
jjmedina	0:a5367bd4e404	673
jjmedina	0:a5367bd4e404	674	if(ensayo_red == false)
jjmedina	0:a5367bd4e404	675	{
jjmedina	0:a5367bd4e404	676	if(green_PUSH.read() == 1)
jjmedina	0:a5367bd4e404	677	{
jjmedina	0:a5367bd4e404	678	datosPC_green = !datosPC_green;
jjmedina	0:a5367bd4e404	679
jjmedina	0:a5367bd4e404	680	if(datosPC_green == true)
jjmedina	0:a5367bd4e404	681	{
jjmedina	0:a5367bd4e404	682	if(TRADUCE == 0)
jjmedina	0:a5367bd4e404	683	{
jjmedina	0:a5367bd4e404	684	display.Clear(); display.SetXY(0,0); display.printf("Aparato en"); display.SetXY(0,1); display.printf("modo PC.");
jjmedina	0:a5367bd4e404	685	wait(1.5);
jjmedina	0:a5367bd4e404	686	display.Clear();
jjmedina	0:a5367bd4e404	687	}
jjmedina	0:a5367bd4e404	688	else
jjmedina	0:a5367bd4e404	689	{
jjmedina	0:a5367bd4e404	690	display.Clear(); display.SetXY(0,0); display.printf("Set in PC mode.");
jjmedina	0:a5367bd4e404	691	wait(1.5);
jjmedina	0:a5367bd4e404	692	display.Clear();
jjmedina	0:a5367bd4e404	693	}
jjmedina	0:a5367bd4e404	694	}
jjmedina	0:a5367bd4e404	695
jjmedina	0:a5367bd4e404	696	if(datosPC_green == false)
jjmedina	0:a5367bd4e404	697	{
jjmedina	0:a5367bd4e404	698	if(TRADUCE == 0)
jjmedina	0:a5367bd4e404	699	{
jjmedina	0:a5367bd4e404	700	display.Clear(); display.SetXY(0,0); display.printf("Aparato en"); display.SetXY(0,1); display.printf("modo autonomo.");
jjmedina	0:a5367bd4e404	701	wait(1.5);
jjmedina	0:a5367bd4e404	702	display.Clear();
jjmedina	0:a5367bd4e404	703	}
jjmedina	0:a5367bd4e404	704	else
jjmedina	0:a5367bd4e404	705	{
jjmedina	0:a5367bd4e404	706	display.Clear(); display.SetXY(0,0); display.printf("Set in"); display.SetXY(0,1); display.printf("autonomous mode.");
jjmedina	0:a5367bd4e404	707	wait(1.5);
jjmedina	0:a5367bd4e404	708	display.Clear();
jjmedina	0:a5367bd4e404	709	}
jjmedina	0:a5367bd4e404	710	}
jjmedina	0:a5367bd4e404	711	}
jjmedina	0:a5367bd4e404	712
jjmedina	0:a5367bd4e404	713	if(datosPC_green == true)
jjmedina	0:a5367bd4e404	714	{
jjmedina	0:a5367bd4e404	715	green_LED = 1;
jjmedina	0:a5367bd4e404	716	}
jjmedina	0:a5367bd4e404	717	else
jjmedina	0:a5367bd4e404	718	{
jjmedina	0:a5367bd4e404	719	green_LED = 0;
jjmedina	0:a5367bd4e404	720	}//enciende o apaga LED verde
jjmedina	0:a5367bd4e404	721	}
jjmedina	0:a5367bd4e404	722
jjmedina	0:a5367bd4e404	723	/////////////////////////////
jjmedina	0:a5367bd4e404	724	// MEDIDA DE ACELERACIONES //
jjmedina	0:a5367bd4e404	725	/////////////////////////////
jjmedina	0:a5367bd4e404	726
jjmedina	0:a5367bd4e404	727	if(ensayo_red == true)
jjmedina	0:a5367bd4e404	728	{
jjmedina	0:a5367bd4e404	729
jjmedina	0:a5367bd4e404	730	if(datosPC_green == true)//Manda los datos al PC, puerto serie
jjmedina	0:a5367bd4e404	731	{
jjmedina	0:a5367bd4e404	732	////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	733	/// LECTURA CON ACELEROMETRO BMA180, DIGITAL ///
jjmedina	0:a5367bd4e404	734	////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	735
jjmedina	0:a5367bd4e404	736	bma180.ReadAccels_BMA180(readings);
jjmedina	0:a5367bd4e404	737
jjmedina	0:a5367bd4e404	738	int16_t aaX = (int16_t)readings[0];
jjmedina	0:a5367bd4e404	739	int16_t aaY = (int16_t)readings[1];
jjmedina	0:a5367bd4e404	740	int16_t aaZ = (int16_t)readings[2];
jjmedina	0:a5367bd4e404	741
jjmedina	0:a5367bd4e404	742	aX = (-1.0)((float)aaX-(-103.23))0.0018;
jjmedina	0:a5367bd4e404	743	aY = (-1.0)((float)aaY-(57.21))0.0018;
jjmedina	0:a5367bd4e404	744	aZ = (-1.0)((float)aaZ-(701.1184))0.0018097570403;
jjmedina	0:a5367bd4e404	745
jjmedina	0:a5367bd4e404	746	ContadorMedidas++;
jjmedina	0:a5367bd4e404	747
jjmedina	0:a5367bd4e404	748	pc.printf("%d,%f,%f,%f\n", timer.read_us(), aX, aY, aZ);
jjmedina	0:a5367bd4e404	749	}
jjmedina	0:a5367bd4e404	750
jjmedina	0:a5367bd4e404	751
jjmedina	0:a5367bd4e404	752	if(datosPC_green == false)//Manda los datos a fichero USB y promedia medidas perdidas
jjmedina	0:a5367bd4e404	753	{
jjmedina	0:a5367bd4e404	754
jjmedina	0:a5367bd4e404	755	GuardaDatosBuffer();
jjmedina	0:a5367bd4e404	756
jjmedina	0:a5367bd4e404	757	/*
jjmedina	0:a5367bd4e404	758	////////////////////////////////////
jjmedina	0:a5367bd4e404	759	// PROMEDIO MEDIDAS ACELERACIONES //
jjmedina	0:a5367bd4e404	760	////////////////////////////////////
jjmedina	0:a5367bd4e404	761
jjmedina	0:a5367bd4e404	762	if(ContadorMedidas == 1) {TiempoInicio = timer.read_us();}
jjmedina	0:a5367bd4e404	763
jjmedina	0:a5367bd4e404	764	TiempoFin = timer.read_us();
jjmedina	0:a5367bd4e404	765
jjmedina	0:a5367bd4e404	766	if((TiempoFin-TiempoInicio)<((ContadorDatos+1)*PasoTiempo))
jjmedina	0:a5367bd4e404	767	{
jjmedina	0:a5367bd4e404	768	nDatosPorPaso++;
jjmedina	0:a5367bd4e404	769	aXm += aX; aYm += aY; aZm += aZ;
jjmedina	0:a5367bd4e404	770	}
jjmedina	0:a5367bd4e404	771	else
jjmedina	0:a5367bd4e404	772	{
jjmedina	0:a5367bd4e404	773	//Calculo del numero de pasos en blanco
jjmedina	0:a5367bd4e404	774	int PasosEnBlanco = ((TiempoFin-TiempoInicio)-(ContadorDatos+1)*PasoTiempo)/(PasoTiempo);
jjmedina	0:a5367bd4e404	775
jjmedina	0:a5367bd4e404	776	if(PasosEnBlanco == 0)
jjmedina	0:a5367bd4e404	777	{
jjmedina	0:a5367bd4e404	778	fprintf(fp,"%d %f %f %f %d\n", ContadorDatos*PasoTiempo, aXm/(float)nDatosPorPaso, aYm/(float)nDatosPorPaso, aZm/(float)nDatosPorPaso, nDatosPorPaso);
jjmedina	0:a5367bd4e404	779	aXm = aX; aYm = aY; aZm = aZ;
jjmedina	0:a5367bd4e404	780	nDatosPorPaso = 1;
jjmedina	0:a5367bd4e404	781	ContadorDatos ++;
jjmedina	0:a5367bd4e404	782	}
jjmedina	0:a5367bd4e404	783	else
jjmedina	0:a5367bd4e404	784	{
jjmedina	0:a5367bd4e404	785	for(int i = 0; i<PasosEnBlanco; i++)
jjmedina	0:a5367bd4e404	786	{
jjmedina	0:a5367bd4e404	787	fprintf(fp,"%d %f %f %f %d\n", (ContadorDatos+i)*PasoTiempo, -20.0, -20.0, -20.0, 0);
jjmedina	0:a5367bd4e404	788	//fprintf(fp,"t = %d, PasosEnblanco = %d, FinalMedida = %d \n", (ContadorDatos+i)*PasoTiempo, PasosEnBlanco,TiempoFin-TiempoInicio);
jjmedina	0:a5367bd4e404	789
jjmedina	0:a5367bd4e404	790	aXm = aX; aYm = aY; aZm = aZ;
jjmedina	0:a5367bd4e404	791	}
jjmedina	0:a5367bd4e404	792	nDatosPorPaso = 1;
jjmedina	0:a5367bd4e404	793	ContadorDatos = ContadorDatos + PasosEnBlanco;
jjmedina	0:a5367bd4e404	794	}
jjmedina	0:a5367bd4e404	795	}*/
jjmedina	0:a5367bd4e404	796	}
jjmedina	0:a5367bd4e404	797	}
jjmedina	0:a5367bd4e404	798	}
jjmedina	0:a5367bd4e404	799	}
jjmedina	0:a5367bd4e404	800
jjmedina	0:a5367bd4e404	801
jjmedina	0:a5367bd4e404	802	void InterpolaMedidasPerdidas(unsigned int ContadorEnsayos, unsigned int ContadorDatos)
jjmedina	0:a5367bd4e404	803	{
jjmedina	0:a5367bd4e404	804	int t;
jjmedina	0:a5367bd4e404	805	float aXYZ[3];
jjmedina	0:a5367bd4e404	806	float aXYZantes[3];
jjmedina	0:a5367bd4e404	807	float aXYZdespues[3];
jjmedina	0:a5367bd4e404	808	float aXYZinterpolada[3];
jjmedina	0:a5367bd4e404	809	int tPasosPerdidos[300];
jjmedina	0:a5367bd4e404	810
jjmedina	0:a5367bd4e404	811	FILE * fp;
jjmedina	0:a5367bd4e404	812	FILE * fp2;
jjmedina	0:a5367bd4e404	813
jjmedina	0:a5367bd4e404	814	char NoViajel[2];
jjmedina	0:a5367bd4e404	815	char resultsFile[50]; char interpolatedFile[50];
jjmedina	0:a5367bd4e404	816
jjmedina	0:a5367bd4e404	817	strcpy(resultsFile,"/local/raw_V_"); strcpy(interpolatedFile,"/local/IV_");
jjmedina	0:a5367bd4e404	818
jjmedina	0:a5367bd4e404	819	sprintf (NoViajel, "%d", ContadorEnsayos);
jjmedina	0:a5367bd4e404	820	strcat(resultsFile,NoViajel); strcat(interpolatedFile,NoViajel);
jjmedina	0:a5367bd4e404	821	strcat(resultsFile,".txt"); strcat(interpolatedFile,".dat");
jjmedina	0:a5367bd4e404	822
jjmedina	0:a5367bd4e404	823	fp = fopen(resultsFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", resultsFile); }
jjmedina	0:a5367bd4e404	824	fp2 = fopen(interpolatedFile,"w"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", interpolatedFile); }
jjmedina	0:a5367bd4e404	825
jjmedina	0:a5367bd4e404	826	unsigned int i = 0;
jjmedina	0:a5367bd4e404	827	unsigned int nDatosPorPaso;
jjmedina	0:a5367bd4e404	828	unsigned int PasosPerdidos = 0;
jjmedina	0:a5367bd4e404	829
jjmedina	0:a5367bd4e404	830	while(i<ContadorDatos)
jjmedina	0:a5367bd4e404	831	{
jjmedina	0:a5367bd4e404	832	fscanf(fp,"%d %f %f %f %d", &t, &aXYZ[0], &aXYZ[1], &aXYZ[2], &nDatosPorPaso);
jjmedina	0:a5367bd4e404	833
jjmedina	0:a5367bd4e404	834
jjmedina	0:a5367bd4e404	835
jjmedina	0:a5367bd4e404	836	if(nDatosPorPaso >0)
jjmedina	0:a5367bd4e404	837	{
jjmedina	0:a5367bd4e404	838	aXYZantes[0] = aXYZ[0]; aXYZantes[1] = aXYZ[1]; aXYZantes[2] = aXYZ[2];
jjmedina	0:a5367bd4e404	839	fprintf(fp2,"%f %f %f %f\n", (float)t/1000000.0, aXYZ[0], aXYZ[1], aXYZ[2]);
jjmedina	0:a5367bd4e404	840	i++;
jjmedina	0:a5367bd4e404	841	}
jjmedina	0:a5367bd4e404	842	else
jjmedina	0:a5367bd4e404	843	{
jjmedina	0:a5367bd4e404	844	tPasosPerdidos[PasosPerdidos] = t;
jjmedina	0:a5367bd4e404	845	PasosPerdidos = 1;
jjmedina	0:a5367bd4e404	846
jjmedina	0:a5367bd4e404	847	while(nDatosPorPaso == 0)
jjmedina	0:a5367bd4e404	848	{
jjmedina	0:a5367bd4e404	849	fscanf(fp,"%d %f %f %f %d", &t, &aXYZ[0], &aXYZ[1], &aXYZ[2], &nDatosPorPaso);
jjmedina	0:a5367bd4e404	850
jjmedina	0:a5367bd4e404	851	if(nDatosPorPaso == 0)
jjmedina	0:a5367bd4e404	852	{
jjmedina	0:a5367bd4e404	853	tPasosPerdidos[PasosPerdidos] = t;
jjmedina	0:a5367bd4e404	854	PasosPerdidos++;
jjmedina	0:a5367bd4e404	855	}
jjmedina	0:a5367bd4e404	856	else
jjmedina	0:a5367bd4e404	857	{
jjmedina	0:a5367bd4e404	858	aXYZdespues[0] = aXYZ[0]; aXYZdespues[1] = aXYZ[1]; aXYZdespues[2] = aXYZ[2];
jjmedina	0:a5367bd4e404	859	}
jjmedina	0:a5367bd4e404	860	}
jjmedina	0:a5367bd4e404	861
jjmedina	0:a5367bd4e404	862	for(unsigned int j = 0; j<PasosPerdidos; j++)
jjmedina	0:a5367bd4e404	863	{
jjmedina	0:a5367bd4e404	864	//aXYZinterpolada[0] = aXYZantes[0]+((aXYZdespues[0]-aXYZantes[0])/(float(PasosPerdidos+1)))*(float)(j+1);
jjmedina	0:a5367bd4e404	865	//aXYZinterpolada[1] = aXYZantes[1]+((aXYZdespues[1]-aXYZantes[1])/(float(PasosPerdidos+1)))*(float)(j+1);
jjmedina	0:a5367bd4e404	866	//aXYZinterpolada[2] = aXYZantes[2]+((aXYZdespues[2]-aXYZantes[2])/(float(PasosPerdidos+1)))*(float)(j+1);
jjmedina	0:a5367bd4e404	867
jjmedina	0:a5367bd4e404	868	aXYZinterpolada[0] = (aXYZdespues[0]+aXYZantes[0])/2.0;
jjmedina	0:a5367bd4e404	869	aXYZinterpolada[1] = (aXYZdespues[1]+aXYZantes[1])/2.0;
jjmedina	0:a5367bd4e404	870	aXYZinterpolada[2] = (aXYZdespues[2]+aXYZantes[2])/2.0;
jjmedina	0:a5367bd4e404	871
jjmedina	0:a5367bd4e404	872	fprintf(fp2,"%f %f %f %f\n", (float)tPasosPerdidos[j]/1000000.0, aXYZinterpolada[0], aXYZinterpolada[1], aXYZinterpolada[2]);
jjmedina	0:a5367bd4e404	873	i++;
jjmedina	0:a5367bd4e404	874	}
jjmedina	0:a5367bd4e404	875	fprintf(fp2,"%f %f %f %f\n", (float)t/1000000.0, aXYZ[0], aXYZ[1], aXYZ[2]);
jjmedina	0:a5367bd4e404	876	i++;
jjmedina	0:a5367bd4e404	877	PasosPerdidos = 0;
jjmedina	0:a5367bd4e404	878	}
jjmedina	0:a5367bd4e404	879	}
jjmedina	0:a5367bd4e404	880
jjmedina	0:a5367bd4e404	881	fclose(fp);
jjmedina	0:a5367bd4e404	882	fclose(fp2);
jjmedina	0:a5367bd4e404	883
jjmedina	0:a5367bd4e404	884	//elimina fichero RAW
jjmedina	0:a5367bd4e404	885	int rm = remove(resultsFile);
jjmedina	0:a5367bd4e404	886	}
jjmedina	0:a5367bd4e404	887
jjmedina	0:a5367bd4e404	888
jjmedina	0:a5367bd4e404	889	void FiltradoPeriodosIntegracion(unsigned int ContadorEnsayos, unsigned int ContadorDatos)
jjmedina	0:a5367bd4e404	890	{
jjmedina	0:a5367bd4e404	891	float t;
jjmedina	0:a5367bd4e404	892	float aXYZ[3]; //vector aceleracion en el paso i [aX,aY,aZ]
jjmedina	0:a5367bd4e404	893
jjmedina	0:a5367bd4e404	894	float xv[3]; //Para hacer las operaciones del Filtro Butterworth
jjmedina	0:a5367bd4e404	895	float yv[3]; //Para hacer las operaciones del Filtro Butterworth
jjmedina	0:a5367bd4e404	896
jjmedina	0:a5367bd4e404	897	const float tol = 0.05; //Porcentaje en que la aceleracion debe superar la media para considerar que cambia el periodo del viaje
jjmedina	0:a5367bd4e404	898
jjmedina	0:a5367bd4e404	899	bool mov = false; //Indica si hay o no movimiento
jjmedina	0:a5367bd4e404	900	bool velV = false; //Indica si estamos en periodo de velocidad constante
jjmedina	0:a5367bd4e404	901	bool decel = false; //Indica si ha empezado la deceleracion
jjmedina	0:a5367bd4e404	902	bool fin = false; //Indica si ha terminado el viaje
jjmedina	0:a5367bd4e404	903	//float t_Arr, t_V0, t_Vfin, t_Par; //Instantes en que empiezan arranque, ...
jjmedina	0:a5367bd4e404	904
jjmedina	0:a5367bd4e404	905	float xZ = 0.0; //Posici�n vertical calculada integrando vZ
jjmedina	0:a5367bd4e404	906	float vZ = 0.0; //Velocidad vertical calculada integrando la aceleraciOn filtrada menos su media
jjmedina	0:a5367bd4e404	907
jjmedina	0:a5367bd4e404	908	float t_3[3], aZ_3[3], vZ_3[3], xZ_3[3]; //Datos guardados para hacer integracion numerica de aceleraciones
jjmedina	0:a5367bd4e404	909	float xa, xb, xc, h, fa, fb, fc, fab, fbc, fabc;
jjmedina	0:a5367bd4e404	910
jjmedina	0:a5367bd4e404	911
jjmedina	0:a5367bd4e404	912	unsigned int nmedV = 50; //Puntos para calcular la media de vZ al final del viaje (que debe ser zero pero no lo es)
jjmedina	0:a5367bd4e404	913
jjmedina	0:a5367bd4e404	914	float sgnAcelArr = 0.0; //Signo de la aceleracion vertical de arranque
jjmedina	0:a5367bd4e404	915	float sgnAcelFr = 0.0; //Signo de la aceleracion vertical de frenada
jjmedina	0:a5367bd4e404	916	float sgnAcelv0 = 0.0; //Signo de la aceleracion vertical al comienzo del periodo de vel cte
jjmedina	0:a5367bd4e404	917	float sgnAcelFin = 0.0; //Signo de la aceleracion vertical al final del periodo de vel cte
jjmedina	0:a5367bd4e404	918
jjmedina	0:a5367bd4e404	919	const float tiempoMinimoArranque = 0.15; //Tiempo mínimo que puede durar la fase de aceleracion de la cabina
jjmedina	0:a5367bd4e404	920	const float tiempoMinimoVelCte = 0.15; //Tiempo mínimo que puede durar el periodo de velocidad constante
jjmedina	0:a5367bd4e404	921	const float tiempoMinimoFrenada = 0.15; //Tiempo mínimo que puede durar el periodo de deceleracion de la cabina
jjmedina	0:a5367bd4e404	922
jjmedina	0:a5367bd4e404	923	for (unsigned int i = 0; i<3; i++) { t_3[i] = 0.0; aZ_3[i] = 0.0; vZ_3[i] = 0.0; xZ_3[i] = 0.0;}
jjmedina	0:a5367bd4e404	924
jjmedina	0:a5367bd4e404	925	FILE * fp;
jjmedina	0:a5367bd4e404	926
jjmedina	0:a5367bd4e404	927	FILE * fp3;
jjmedina	0:a5367bd4e404	928
jjmedina	0:a5367bd4e404	929	char NoViajel[2];
jjmedina	0:a5367bd4e404	930	char resultsFile[50];
jjmedina	0:a5367bd4e404	931	char dispVelFile[50];
jjmedina	0:a5367bd4e404	932
jjmedina	0:a5367bd4e404	933	int dif;
jjmedina	0:a5367bd4e404	934
jjmedina	0:a5367bd4e404	935	//strcpy(resultsFile,"/local/IV_"); strcpy(dispVelFile,"/local/dva0_");
jjmedina	0:a5367bd4e404	936
jjmedina	0:a5367bd4e404	937	strcpy(resultsFile,"/local/raw_V_"); strcpy(dispVelFile,"/local/dva0_");
jjmedina	0:a5367bd4e404	938
jjmedina	0:a5367bd4e404	939	sprintf (NoViajel, "%d", ContadorEnsayos);
jjmedina	0:a5367bd4e404	940
jjmedina	0:a5367bd4e404	941	strcat(resultsFile,NoViajel); strcat(dispVelFile,NoViajel);
jjmedina	0:a5367bd4e404	942	strcat(resultsFile,".txt"); strcat(dispVelFile,".txt");
jjmedina	0:a5367bd4e404	943
jjmedina	0:a5367bd4e404	944	//fichero con todos los datos de la funcion anterior IV_
jjmedina	0:a5367bd4e404	945	fp = fopen(resultsFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open\n"); display.SetXY(0,1); display.printf("%s\n", resultsFile); }
jjmedina	0:a5367bd4e404	946	//fichero donde guardaron
jjmedina	0:a5367bd4e404	947	fp3 = fopen(dispVelFile,"w"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open\n"); display.SetXY(0,1); display.printf("%s\n", dispVelFile); }
jjmedina	0:a5367bd4e404	948
jjmedina	0:a5367bd4e404	949	//Lee, filtra y guarda las tres aceleraciones por separado
jjmedina	0:a5367bd4e404	950
jjmedina	0:a5367bd4e404	951	for(unsigned int i = 0; i<ContadorDatos; i++)
jjmedina	0:a5367bd4e404	952	{
jjmedina	0:a5367bd4e404	953	fscanf(fp,"%f %f %f %f %d", &t, &aXYZ[0], &aXYZ[1], &aXYZ[2], &dif);//lee tiempo y 3 aceleraciones X, Y, Z y valor 'dif' (ver GuardaDatos())
jjmedina	0:a5367bd4e404	954
jjmedina	0:a5367bd4e404	955	///////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	956	/// FILTRO DE BUTTERWORTH, PASO BAJO, ORDEN 2, 10Hz ///
jjmedina	0:a5367bd4e404	957	///////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	958
jjmedina	0:a5367bd4e404	959	if(i < 2)
jjmedina	0:a5367bd4e404	960	{
jjmedina	0:a5367bd4e404	961	xv[i+1] = aXYZ[2];
jjmedina	0:a5367bd4e404	962	yv[i+1] = aXYZ[2];
jjmedina	0:a5367bd4e404	963	}
jjmedina	0:a5367bd4e404	964	else
jjmedina	0:a5367bd4e404	965	{
jjmedina	0:a5367bd4e404	966	xv[0] = xv[1]; xv[1] = xv[2];
jjmedina	0:a5367bd4e404	967	xv[2] = aXYZ[2] / gain;
jjmedina	0:a5367bd4e404	968
jjmedina	0:a5367bd4e404	969	yv[0] = yv[1]; yv[1] = yv[2];
jjmedina	0:a5367bd4e404	970	yv[2] = (xv[0] + xv[2]) + 2.0 * xv[1]
jjmedina	0:a5367bd4e404	971	+ ( c_butt1 * yv[0]) + ( c_butt2 * yv[1]);
jjmedina	0:a5367bd4e404	972	aXYZ[2] = yv[2];
jjmedina	0:a5367bd4e404	973	}
jjmedina	0:a5367bd4e404	974
jjmedina	0:a5367bd4e404	975	/////////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	976	/// DETECCION INSTANTES DE ARRANQUE, COMIENZO Vcte, FIN Vcte Y PARADA ///
jjmedina	0:a5367bd4e404	977	/////////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	978
jjmedina	0:a5367bd4e404	979	/// CALCULO DE ACELERACION Z DE REFERENCIA (DEBE COINCIDIR CON LA GRAVEDAD: 9.81) aZm0
jjmedina	0:a5367bd4e404	980
jjmedina	0:a5367bd4e404	981	if( (i >= ndes)&&(i < (ndes+nmed)))
jjmedina	0:a5367bd4e404	982	{
jjmedina	0:a5367bd4e404	983	aXm0 += aXYZ[0]/(float)nmed;
jjmedina	0:a5367bd4e404	984	aYm0 += aXYZ[1]/(float)nmed;
jjmedina	0:a5367bd4e404	985	aZm0 += aXYZ[2]/(float)nmed;
jjmedina	0:a5367bd4e404	986	}
jjmedina	0:a5367bd4e404	987	//Calculo nArr,nV0,nVfin,nPar
jjmedina	0:a5367bd4e404	988	if( (i >= (ndes+nmed)))
jjmedina	0:a5367bd4e404	989	{
jjmedina	0:a5367bd4e404	990	if ((mov == false) && (fin == false))
jjmedina	0:a5367bd4e404	991	{
jjmedina	0:a5367bd4e404	992	if(fabs(aXYZ[2]-aZm0) > fabs(tol*aZm0))
jjmedina	0:a5367bd4e404	993	{
jjmedina	0:a5367bd4e404	994	nArr = i-(ndes+nmed); //Detecta el instante de arranque
jjmedina	0:a5367bd4e404	995	mov = true;
jjmedina	0:a5367bd4e404	996	t_Arr = t;
jjmedina	0:a5367bd4e404	997
jjmedina	0:a5367bd4e404	998	sgnAcelArr = (aXYZ[2]-aZm0)/fabs(aXYZ[2]-aZm0); //Calcula el signo de la aceleracion vertical de arranque
jjmedina	0:a5367bd4e404	999
jjmedina	0:a5367bd4e404	1000	//pc.printf("i = %d, aZ = %f, aZm0 = %f, dif = %f, tol = %f, t = %f\n",i-(ndes+nmed), aXYZ[2],aZm0,aXYZ[2]-aZm0, fabs(tol*aZm0),t);
jjmedina	0:a5367bd4e404	1001	}
jjmedina	0:a5367bd4e404	1002	}
jjmedina	0:a5367bd4e404	1003
jjmedina	0:a5367bd4e404	1004	if ( (mov == true) && (velV == false) && (decel == false) && (t-t_Arr>tiempoMinimoArranque) )
jjmedina	0:a5367bd4e404	1005	{
jjmedina	0:a5367bd4e404	1006	if(fabs(aXYZ[2]-aZm0) < fabs(tol*aZm0))
jjmedina	0:a5367bd4e404	1007	{
jjmedina	0:a5367bd4e404	1008	sgnAcelv0 = (aXYZ[2]-aZm0)/fabs(aXYZ[2]-aZm0);
jjmedina	0:a5367bd4e404	1009	if(fabs(sgnAcelv0-sgnAcelArr)>1.5) //El signo de la aceleracion de arranque tiene que ser distinto del signo de la de frenada
jjmedina	0:a5367bd4e404	1010	{
jjmedina	0:a5367bd4e404	1011	nV0 = i-(ndes+nmed); //Detecta el instante de inicio del periodo de V cte
jjmedina	0:a5367bd4e404	1012	velV = true;
jjmedina	0:a5367bd4e404	1013	t_V0 = t;
jjmedina	0:a5367bd4e404	1014	//pc.printf("i = %d, aZ = %f, aZm0 = %f, dif = %f, tol = %f, t = %f\n",i-(ndes+nmed), aXYZ[2],aZm0,aXYZ[2]-aZm0, fabs(tol*aZm0),t);
jjmedina	0:a5367bd4e404	1015	}
jjmedina	0:a5367bd4e404	1016	}
jjmedina	0:a5367bd4e404	1017	}
jjmedina	0:a5367bd4e404	1018
jjmedina	0:a5367bd4e404	1019	if ( (mov == true) && (velV == true) && (decel == false) && (t-t_V0>tiempoMinimoVelCte))
jjmedina	0:a5367bd4e404	1020	{
jjmedina	0:a5367bd4e404	1021	if(fabs(aXYZ[2]-aZm0) > fabs(tol*aZm0))
jjmedina	0:a5367bd4e404	1022	{
jjmedina	0:a5367bd4e404	1023	sgnAcelFr = (aXYZ[2]-aZm0)/fabs(aXYZ[2]-aZm0);
jjmedina	0:a5367bd4e404	1024
jjmedina	0:a5367bd4e404	1025	if(fabs(sgnAcelFr-sgnAcelArr)>1.5) //El signo de la aceleracion de arranque tiene que ser distinto del signo de la de frenada
jjmedina	0:a5367bd4e404	1026	{
jjmedina	0:a5367bd4e404	1027	nVfin = i-(ndes+nmed); //Detecta el instante de fin del periodo de V cte
jjmedina	0:a5367bd4e404	1028	velV = false;
jjmedina	0:a5367bd4e404	1029	decel = true;
jjmedina	0:a5367bd4e404	1030
jjmedina	0:a5367bd4e404	1031	t_Vfin = t;
jjmedina	0:a5367bd4e404	1032
jjmedina	0:a5367bd4e404	1033	//pc.printf("i = %d, aZ = %f, aZm0 = %f, dif = %f, tol = %f, t = %f\n",i-(ndes+nmed), aXYZ[2],aZm0,aXYZ[2]-aZm0, fabs(tol*aZm0),t);
jjmedina	0:a5367bd4e404	1034	}
jjmedina	0:a5367bd4e404	1035	}
jjmedina	0:a5367bd4e404	1036	}
jjmedina	0:a5367bd4e404	1037
jjmedina	0:a5367bd4e404	1038	if ( (mov == true) && (velV == false) && (decel == true) && (t-t_Vfin>tiempoMinimoFrenada))
jjmedina	0:a5367bd4e404	1039	{
jjmedina	0:a5367bd4e404	1040	if(fabs(aXYZ[2]-aZm0) < fabs(tol*aZm0))
jjmedina	0:a5367bd4e404	1041	{
jjmedina	0:a5367bd4e404	1042	sgnAcelFin = (aXYZ[2]-aZm0)/fabs(aXYZ[2]-aZm0);
jjmedina	0:a5367bd4e404	1043
jjmedina	0:a5367bd4e404	1044	if(fabs(sgnAcelFin-sgnAcelFr)>1.5)
jjmedina	0:a5367bd4e404	1045	{
jjmedina	0:a5367bd4e404	1046	nPar = i-(ndes+nmed); //Detecta el instante de parada
jjmedina	0:a5367bd4e404	1047	velV = false;
jjmedina	0:a5367bd4e404	1048	decel = false;
jjmedina	0:a5367bd4e404	1049	mov = false;
jjmedina	0:a5367bd4e404	1050	fin = true;
jjmedina	0:a5367bd4e404	1051	t_Par = t;
jjmedina	0:a5367bd4e404	1052	//pc.printf("i = %d, aZ = %f, aZm0 = %f, dif = %f, tol = %f, t = %f\n",i-(ndes+nmed), aXYZ[2],aZm0,aXYZ[2]-aZm0, fabs(tol*aZm0),t);
jjmedina	0:a5367bd4e404	1053	}
jjmedina	0:a5367bd4e404	1054	}
jjmedina	0:a5367bd4e404	1055	}
jjmedina	0:a5367bd4e404	1056	}
jjmedina	0:a5367bd4e404	1057
jjmedina	0:a5367bd4e404	1058	////////////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	1059	/// CALCULO DE POSICION Y VELOCIDAD INTEGRANDO LA ACELERACION Z FILTRADA ///
jjmedina	0:a5367bd4e404	1060	////////////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	1061
jjmedina	0:a5367bd4e404	1062	int i2 = i-(ndes+nmed); //Nuevo idice para calcular posicion y velocidad
jjmedina	0:a5367bd4e404	1063
jjmedina	0:a5367bd4e404	1064
jjmedina	0:a5367bd4e404	1065
jjmedina	0:a5367bd4e404	1066	//if (i2 == 0) {t_int = t;} //Ojo con los primeros valores
jjmedina	0:a5367bd4e404	1067
jjmedina	0:a5367bd4e404	1068	//actualizo valores t_3, aZ_3
jjmedina	0:a5367bd4e404	1069	t_3[0] = t_3[1];
jjmedina	0:a5367bd4e404	1070	t_3[1] = t_3[2];
jjmedina	0:a5367bd4e404	1071
jjmedina	0:a5367bd4e404	1072	t_3[2] = t;
jjmedina	0:a5367bd4e404	1073
jjmedina	0:a5367bd4e404	1074	aZ_3[0] = aZ_3[1];
jjmedina	0:a5367bd4e404	1075	aZ_3[1] = aZ_3[2];
jjmedina	0:a5367bd4e404	1076	aZ_3[2] = aXYZ[2]-aZm0;
jjmedina	0:a5367bd4e404	1077
jjmedina	0:a5367bd4e404	1078	if(i2 >= 0)
jjmedina	0:a5367bd4e404	1079	{
jjmedina	0:a5367bd4e404	1080	vZ_3[0] = vZ_3[1];
jjmedina	0:a5367bd4e404	1081	vZ_3[1] = vZ_3[2];
jjmedina	0:a5367bd4e404	1082
jjmedina	0:a5367bd4e404	1083	xZ_3[0] = xZ_3[1];
jjmedina	0:a5367bd4e404	1084	xZ_3[1] = xZ_3[2];
jjmedina	0:a5367bd4e404	1085
jjmedina	0:a5367bd4e404	1086	xa = t_3[2];
jjmedina	0:a5367bd4e404	1087	xb = t_3[1];
jjmedina	0:a5367bd4e404	1088	xc = t_3[0];
jjmedina	0:a5367bd4e404	1089
jjmedina	0:a5367bd4e404	1090	h = (xa-xb)/2.0;
jjmedina	0:a5367bd4e404	1091
jjmedina	0:a5367bd4e404	1092	fa = aZ_3[2];
jjmedina	0:a5367bd4e404	1093	fb = aZ_3[1];
jjmedina	0:a5367bd4e404	1094	fc = aZ_3[0];
jjmedina	0:a5367bd4e404	1095
jjmedina	0:a5367bd4e404	1096	//if(fabs(xa-xb)>1.0e-9) {fab = (fa-fb)/(xa-xb);} else {fab = 0.0;}
jjmedina	0:a5367bd4e404	1097	//if(fabs(xb-xc)>1.0e-9) {fbc = (fb-fc)/(xb-xc);} else {fbc = 0.0;}
jjmedina	0:a5367bd4e404	1098	//if(fabs(xa-xc)>1.0e-9) {fabc = (fab-fbc)/(xa-xc);} else {fabc = 0.0;}
jjmedina	0:a5367bd4e404	1099	fab = (fa-fb)/(xa-xb);
jjmedina	0:a5367bd4e404	1100	fbc = (fb-fc)/(xb-xc);
jjmedina	0:a5367bd4e404	1101	fabc = (fab-fbc)/(xa-xc);
jjmedina	0:a5367bd4e404	1102
jjmedina	0:a5367bd4e404	1103	vZ_3[2]= vZ_3[1] + fa2.0h + fab(-2.0hh) + fabc(-4.0hh*h/6.0);
jjmedina	0:a5367bd4e404	1104
jjmedina	0:a5367bd4e404	1105	// Posicion
jjmedina	0:a5367bd4e404	1106
jjmedina	0:a5367bd4e404	1107	fa = vZ_3[2];
jjmedina	0:a5367bd4e404	1108	fb = vZ_3[1];
jjmedina	0:a5367bd4e404	1109	fc = vZ_3[0];
jjmedina	0:a5367bd4e404	1110
jjmedina	0:a5367bd4e404	1111	//if(fabs(xa-xb)>1.0e-9) {fab = (fa-fb)/(xa-xb);} else {fab = 0.0;}
jjmedina	0:a5367bd4e404	1112	//if(fabs(xb-xc)>1.0e-9) {fbc = (fb-fc)/(xb-xc);} else {fbc = 0.0;}
jjmedina	0:a5367bd4e404	1113	//if(fabs(xa-xc)>1.0e-9) {fabc = (fab-fbc)/(xa-xc);} else {fabc = 0.0;}
jjmedina	0:a5367bd4e404	1114	fab = (fa-fb)/(xa-xb);
jjmedina	0:a5367bd4e404	1115	fbc = (fb-fc)/(xb-xc);
jjmedina	0:a5367bd4e404	1116	fabc = (fab-fbc)/(xa-xc);
jjmedina	0:a5367bd4e404	1117
jjmedina	0:a5367bd4e404	1118	xZ_3[2] = xZ_3[1] + fa2.0h + fab(-2.0hh) + fabc(-4.0hh*h/6.0);
jjmedina	0:a5367bd4e404	1119
jjmedina	0:a5367bd4e404	1120	fprintf(fp3,"%f %f %f %f \n", t_3[2], xZ_3[2], vZ_3[2], aZ_3[2]);//no guardo aX ni aY, los tengo en IV_i
jjmedina	0:a5367bd4e404	1121	}
jjmedina	0:a5367bd4e404	1122
jjmedina	0:a5367bd4e404	1123	//CALCULO DE LOS DESPLAZAMIENTOS EN INSTANTES DE ARRANQUE Y PARADA
jjmedina	0:a5367bd4e404	1124
jjmedina	0:a5367bd4e404	1125	if(i-(ndes+nmed) == nArr) {d_arr = xZ_3[2];}
jjmedina	0:a5367bd4e404	1126	if(i-(ndes+nmed) == nPar) {d_par = xZ_3[2];}
jjmedina	0:a5367bd4e404	1127
jjmedina	0:a5367bd4e404	1128	//CALCULO VELOCIDAD MEDIA EN INSTANTES FINALES
jjmedina	0:a5367bd4e404	1129	//PARA IMPLEMENTAR EL TRUCO DE DANI
jjmedina	0:a5367bd4e404	1130
jjmedina	0:a5367bd4e404	1131	if((ContadorDatos-i) <= nmedV)
jjmedina	0:a5367bd4e404	1132	{
jjmedina	0:a5367bd4e404	1133	vZmFin += vZ_3[2]/(float)nmedV;
jjmedina	0:a5367bd4e404	1134	tFin = t_3[2];
jjmedina	0:a5367bd4e404	1135	}
jjmedina	0:a5367bd4e404	1136
jjmedina	0:a5367bd4e404	1137	//Calculo de las velociades obtenidas por integración (que deben ser 0 en realidad) en los instantes de arranque y parada
jjmedina	0:a5367bd4e404	1138
jjmedina	0:a5367bd4e404	1139	if(nArr == (i-(ndes+nmed))) {vArr = vZ_3[2];}
jjmedina	0:a5367bd4e404	1140	if(nPar == (i-(ndes+nmed))) {vPar = vZ_3[2];}
jjmedina	0:a5367bd4e404	1141
jjmedina	0:a5367bd4e404	1142
jjmedina	0:a5367bd4e404	1143	}//Fin loop principal
jjmedina	0:a5367bd4e404	1144
jjmedina	0:a5367bd4e404	1145
jjmedina	0:a5367bd4e404	1146
jjmedina	0:a5367bd4e404	1147	fclose(fp);
jjmedina	0:a5367bd4e404	1148
jjmedina	0:a5367bd4e404	1149	fclose(fp3);
jjmedina	0:a5367bd4e404	1150
jjmedina	0:a5367bd4e404	1151
jjmedina	0:a5367bd4e404	1152
jjmedina	0:a5367bd4e404	1153	}
jjmedina	0:a5367bd4e404	1154
jjmedina	0:a5367bd4e404	1155	void DatosConfort(unsigned int ContadorEnsayos, unsigned int ContadorDatos, int PasoTiempo)
jjmedina	0:a5367bd4e404	1156	{
jjmedina	0:a5367bd4e404	1157	float t;
jjmedina	0:a5367bd4e404	1158	float dZ, vZ, aZ;
jjmedina	0:a5367bd4e404	1159
jjmedina	0:a5367bd4e404	1160	const float tolV = 0.01; //Tolerancia, en tanto por uno de la velocidad nominal, para recalcular el tiempo final del viaje
jjmedina	0:a5367bd4e404	1161
jjmedina	0:a5367bd4e404	1162	unsigned int RecalculaNPar = 0; //Se pone a 1 para recalcular el instante de parada
jjmedina	0:a5367bd4e404	1163	unsigned int CalculaBo1 = 0; //Se pone a 1 para calcular el limite Bo1
jjmedina	0:a5367bd4e404	1164	unsigned int CalculaBo2 = 0; //Se pone a 1 para calcular el limite Bo2
jjmedina	0:a5367bd4e404	1165
jjmedina	0:a5367bd4e404	1166	Bo0 = 0; //Se establece el Bo0 al principio, tras los puntos descartados y los de calculo de g
jjmedina	0:a5367bd4e404	1167
jjmedina	0:a5367bd4e404	1168	Bo3 = ContadorDatos-(ndes+nmed)-1; //Se establece el Bo3 al final, contando los puntos descartados y los de calculo de g
jjmedina	0:a5367bd4e404	1169	if(Bo3>4000000000){Bo3 = 0;}
jjmedina	0:a5367bd4e404	1170
jjmedina	0:a5367bd4e404	1171	Boacel = (unsigned int)(nV0 + nVfin)*0.5; //Se establece inicialmente (luego se vera si tenemos que cambiarlo) como la media entre nV0 y nV
jjmedina	0:a5367bd4e404	1172
jjmedina	0:a5367bd4e404	1173	//float amax = 0.0;//VALOR POR PANTALLA
jjmedina	0:a5367bd4e404	1174	//float dmax = 0.0;//VALOR POR PANTALLA
jjmedina	0:a5367bd4e404	1175	//float Vmax = 0.0;//VALOR POR PANTALLA
jjmedina	0:a5367bd4e404	1176
jjmedina	0:a5367bd4e404	1177	//Relativos al calculo del jerk
jjmedina	0:a5367bd4e404	1178	const float running_time = 0.5;
jjmedina	0:a5367bd4e404	1179	int Ai = (int)(0.5running_time1.0e6)/PasoTiempo;
jjmedina	0:a5367bd4e404	1180	float aZ_05s[2*Ai+1]; //Aceleraciones dentro del periodo de 0.5 segundos para hacer regresion lineal
jjmedina	0:a5367bd4e404	1181	float jerk_025s[Ai+1]; //Jerks dentro del periodo de 0.25
jjmedina	0:a5367bd4e404	1182	for (unsigned int i = 0; i<(2*Ai+1); i++) { aZ_05s[i] = 0.0; }
jjmedina	0:a5367bd4e404	1183	for (unsigned int i = 0; i<(Ai+1); i++) { jerk_025s[i] = 0.0; }
jjmedina	0:a5367bd4e404	1184
jjmedina	0:a5367bd4e404	1185	//////////////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	1186	// Datos para nueva integración de la velocidad para obtener desplazamiento //
jjmedina	0:a5367bd4e404	1187	//////////////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	1188
jjmedina	0:a5367bd4e404	1189	float t_3[3], vZ_3[3], xZ_3[3]; //Datos guardados para hacer integracion numerica de aceleraciones
jjmedina	0:a5367bd4e404	1190	float xa, xb, xc, h, fa, fb, fc, fab, fbc, fabc;
jjmedina	0:a5367bd4e404	1191
jjmedina	0:a5367bd4e404	1192
jjmedina	0:a5367bd4e404	1193	for (unsigned int i = 0; i<3; i++)
jjmedina	0:a5367bd4e404	1194	{
jjmedina	0:a5367bd4e404	1195	t_3[i] = 0.0; vZ_3[i] = 0.0; xZ_3[i] = 0.0;
jjmedina	0:a5367bd4e404	1196	}
jjmedina	0:a5367bd4e404	1197	//nuevo Juan, soluciona error en nuevo calculo de dZ
jjmedina	0:a5367bd4e404	1198	t_3[2] = (ndes + nmed - 1)*PasoTiempo/1000000.0;
jjmedina	0:a5367bd4e404	1199	t_3[1] = (ndes + nmed - 2)*PasoTiempo/1000000.0;
jjmedina	0:a5367bd4e404	1200
jjmedina	0:a5367bd4e404	1201	//Archivos de lectura y escritura
jjmedina	0:a5367bd4e404	1202
jjmedina	0:a5367bd4e404	1203	FILE * fp;
jjmedina	0:a5367bd4e404	1204	FILE * fp2;
jjmedina	0:a5367bd4e404	1205
jjmedina	0:a5367bd4e404	1206	char NoViajel[2];
jjmedina	0:a5367bd4e404	1207	char dispVelFile[50]; char velMod[50];
jjmedina	0:a5367bd4e404	1208
jjmedina	0:a5367bd4e404	1209	strcpy(dispVelFile,"/local/dva0_"); strcpy(velMod,"/local/dvaj_");
jjmedina	0:a5367bd4e404	1210
jjmedina	0:a5367bd4e404	1211	sprintf (NoViajel, "%d", ContadorEnsayos);
jjmedina	0:a5367bd4e404	1212	strcat(dispVelFile,NoViajel); strcat(velMod,NoViajel);
jjmedina	0:a5367bd4e404	1213	strcat(dispVelFile,".txt"); strcat(velMod,".txt");
jjmedina	0:a5367bd4e404	1214
jjmedina	0:a5367bd4e404	1215	fp = fopen(dispVelFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", dispVelFile); }
jjmedina	0:a5367bd4e404	1216	fp2 = fopen(velMod,"w"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", velMod); }
jjmedina	0:a5367bd4e404	1217
jjmedina	0:a5367bd4e404	1218
jjmedina	0:a5367bd4e404	1219	//Lee velocidad para establecer periodos y recalcular nPar. Tambnien se calculan Bo1 y Bo2
jjmedina	0:a5367bd4e404	1220
jjmedina	0:a5367bd4e404	1221	int nDatos = (ContadorDatos-(ndes+nmed));
jjmedina	0:a5367bd4e404	1222	if (nDatos < 0) {nDatos = 0;}
jjmedina	0:a5367bd4e404	1223
jjmedina	0:a5367bd4e404	1224	float t_in;
jjmedina	0:a5367bd4e404	1225	float a_LV = (vZmFin-vArr)/(tFin-t_Arr); //Coeficiente de la recta que se usa para llevar a 0 la velocidad integrada en los instantes de arranque y parada
jjmedina	0:a5367bd4e404	1226	float b_LV = vArr - a_LV*t_Arr; //Coeficiente de la recta que se usa para llevar a 0 la velocidad integrada en los instantes de arranque y parada
jjmedina	0:a5367bd4e404	1227
jjmedina	0:a5367bd4e404	1228	for(int i = 0; i < nDatos; i++)
jjmedina	0:a5367bd4e404	1229	{
jjmedina	0:a5367bd4e404	1230	fscanf(fp,"%f %f %f %f", &t, &dZ, &vZ, &aZ);//aZ filtered, aX y aY raw.
jjmedina	0:a5367bd4e404	1231
jjmedina	0:a5367bd4e404	1232	if(i == 0) { t_in = t;}
jjmedina	0:a5367bd4e404	1233
jjmedina	0:a5367bd4e404	1234	//vZ = vZ - vZmFin*(t/tFin); //CAMBIO JL (11/10/2013) CON ESTA FORMA LA VELOCIDAD NO PARTE DE 0
jjmedina	0:a5367bd4e404	1235	//vZ = vZ - vZmFin*((t-t_in)/(tFin-t_in));
jjmedina	0:a5367bd4e404	1236
jjmedina	0:a5367bd4e404	1237	//Truco de Dani mas sofisticado
jjmedina	0:a5367bd4e404	1238	if(i < nArr) { vZ = vZ - vArr*((t-t_in)/(t_Arr-t_in));}
jjmedina	0:a5367bd4e404	1239	if(i >= nArr) { vZ = vZ - (a_LV*t + b_LV);}
jjmedina	0:a5367bd4e404	1240
jjmedina	0:a5367bd4e404	1241
jjmedina	0:a5367bd4e404	1242	// NUEVA INTEGRACIÓN DE LA VELOCIDAD PARA CALCULAR DESPLAZAMIENTO
jjmedina	0:a5367bd4e404	1243
jjmedina	0:a5367bd4e404	1244	t_3[0] = t_3[1];
jjmedina	0:a5367bd4e404	1245	t_3[1] = t_3[2];
jjmedina	0:a5367bd4e404	1246	t_3[2] = t;
jjmedina	0:a5367bd4e404	1247
jjmedina	0:a5367bd4e404	1248	vZ_3[0] = vZ_3[1];
jjmedina	0:a5367bd4e404	1249	vZ_3[1] = vZ_3[2];
jjmedina	0:a5367bd4e404	1250	vZ_3[2] = vZ;
jjmedina	0:a5367bd4e404	1251
jjmedina	0:a5367bd4e404	1252	xZ_3[0] = xZ_3[1];
jjmedina	0:a5367bd4e404	1253	xZ_3[1] = xZ_3[2];
jjmedina	0:a5367bd4e404	1254
jjmedina	0:a5367bd4e404	1255	xa = t_3[2];
jjmedina	0:a5367bd4e404	1256	xb = t_3[1];
jjmedina	0:a5367bd4e404	1257	xc = t_3[0];
jjmedina	0:a5367bd4e404	1258
jjmedina	0:a5367bd4e404	1259	h = (xa-xb)/2.0;
jjmedina	0:a5367bd4e404	1260
jjmedina	0:a5367bd4e404	1261	fa = vZ_3[2];
jjmedina	0:a5367bd4e404	1262	fb = vZ_3[1];
jjmedina	0:a5367bd4e404	1263	fc = vZ_3[0];
jjmedina	0:a5367bd4e404	1264
jjmedina	0:a5367bd4e404	1265	//if(fabs(xa-xb)>1.0e-9) {fab = (fa-fb)/(xa-xb);} else {fab = 0.0;}
jjmedina	0:a5367bd4e404	1266	//if(fabs(xb-xc)>1.0e-9) {fbc = (fb-fc)/(xb-xc);} else {fbc = 0.0;}
jjmedina	0:a5367bd4e404	1267	//if(fabs(xa-xc)>1.0e-9) {fabc = (fab-fbc)/(xa-xc);} else {fabc = 0.0;}
jjmedina	0:a5367bd4e404	1268	fab = (fa-fb)/(xa-xb);
jjmedina	0:a5367bd4e404	1269	fbc = (fb-fc)/(xb-xc);
jjmedina	0:a5367bd4e404	1270	fabc = (fab-fbc)/(xa-xc);
jjmedina	0:a5367bd4e404	1271
jjmedina	0:a5367bd4e404	1272	xZ_3[2] = xZ_3[1] + fa2.0h + fab(-2.0hh) + fabc(-4.0hh*h/6.0);
jjmedina	0:a5367bd4e404	1273
jjmedina	0:a5367bd4e404	1274	dZ = xZ_3[2];
jjmedina	0:a5367bd4e404	1275
jjmedina	0:a5367bd4e404	1276	if(i == nArr) {d_arr = dZ;}
jjmedina	0:a5367bd4e404	1277	if(i == nPar) {d_par = dZ;}
jjmedina	0:a5367bd4e404	1278
jjmedina	0:a5367bd4e404	1279	// HASTA AQUÍ
jjmedina	0:a5367bd4e404	1280
jjmedina	0:a5367bd4e404	1281
jjmedina	0:a5367bd4e404	1282	if( (i>=nV0)&&(i<nVfin) )
jjmedina	0:a5367bd4e404	1283	{
jjmedina	0:a5367bd4e404	1284	vNom += fabs(vZ)/(float)(nVfin-nV0);
jjmedina	0:a5367bd4e404	1285	}
jjmedina	0:a5367bd4e404	1286
jjmedina	0:a5367bd4e404	1287	if( (i == nPar) && (fabs(vZ) > tolV*fabs(vNom)) )
jjmedina	0:a5367bd4e404	1288	{
jjmedina	0:a5367bd4e404	1289	RecalculaNPar = 1;
jjmedina	0:a5367bd4e404	1290	}
jjmedina	0:a5367bd4e404	1291
jjmedina	0:a5367bd4e404	1292	if( (RecalculaNPar == 1) && (fabs(vZ) < tolV*fabs(vNom)) )
jjmedina	0:a5367bd4e404	1293	{
jjmedina	0:a5367bd4e404	1294	nPar = i;
jjmedina	0:a5367bd4e404	1295	t_Par = t;
jjmedina	0:a5367bd4e404	1296	RecalculaNPar = 0;
jjmedina	0:a5367bd4e404	1297	}
jjmedina	0:a5367bd4e404	1298
jjmedina	0:a5367bd4e404	1299	// Calculo de Bo1
jjmedina	0:a5367bd4e404	1300
jjmedina	0:a5367bd4e404	1301	if( i == nArr) { CalculaBo1 = 1; }
jjmedina	0:a5367bd4e404	1302
jjmedina	0:a5367bd4e404	1303	if ((fabs(fabs(dZ)-fabs(d_arr))>0.5) && (CalculaBo1 == 1))
jjmedina	0:a5367bd4e404	1304	{
jjmedina	0:a5367bd4e404	1305	Bo1 = i;
jjmedina	0:a5367bd4e404	1306	CalculaBo1 = 0;
jjmedina	0:a5367bd4e404	1307	}
jjmedina	0:a5367bd4e404	1308	//OJO: INTENTAR CORREGIR Bo1 CON NUEVO PUNTO DE PARADA arranque
jjmedina	0:a5367bd4e404	1309
jjmedina	0:a5367bd4e404	1310	// Calculo de Bo2
jjmedina	0:a5367bd4e404	1311
jjmedina	0:a5367bd4e404	1312	//OJO: INTENTAR CORREGIR Bo2 CON NUEVO PUNTO DE PARADA nPar
jjmedina	0:a5367bd4e404	1313
jjmedina	0:a5367bd4e404	1314	if( i == nV0) { CalculaBo2 = 1; }
jjmedina	0:a5367bd4e404	1315
jjmedina	0:a5367bd4e404	1316	if ((fabs(fabs(dZ)-fabs(d_par))<0.5) && (CalculaBo2 == 1) && (i>Bo1))
jjmedina	0:a5367bd4e404	1317	{
jjmedina	0:a5367bd4e404	1318	Bo2 = i;
jjmedina	0:a5367bd4e404	1319	CalculaBo2 = 0;
jjmedina	0:a5367bd4e404	1320	}
jjmedina	0:a5367bd4e404	1321
jjmedina	0:a5367bd4e404	1322	//Calculo maxima aceleracion y deceleracion. Calcula Vmax1 y Vmax2. n05Vmaxi y n95Vmaxi en la siguiente pasada.
jjmedina	0:a5367bd4e404	1323
jjmedina	0:a5367bd4e404	1324	if ( (i>=Bo0) && (i<Boacel))
jjmedina	0:a5367bd4e404	1325	{
jjmedina	0:a5367bd4e404	1326	if(fabs(aZ)>amax) { amax = fabs(aZ);}
jjmedina	0:a5367bd4e404	1327	if(fabs(vZ)>Vmax1) { Vmax1 = fabs(vZ);}
jjmedina	0:a5367bd4e404	1328	}
jjmedina	0:a5367bd4e404	1329
jjmedina	0:a5367bd4e404	1330	if ( (i>=Boacel) && (i<=Bo3))
jjmedina	0:a5367bd4e404	1331	{
jjmedina	0:a5367bd4e404	1332	if(fabs(aZ)>dmax) { dmax = fabs(aZ);}
jjmedina	0:a5367bd4e404	1333	if(fabs(vZ)>Vmax2) { Vmax2 = fabs(vZ);}
jjmedina	0:a5367bd4e404	1334	}
jjmedina	0:a5367bd4e404	1335
jjmedina	0:a5367bd4e404	1336	//Calculo de jerk por regresion
jjmedina	0:a5367bd4e404	1337
jjmedina	0:a5367bd4e404	1338	for(unsigned int j = 0; j<(2*Ai+1); j++) { aZ_05s[j] = aZ_05s[j+1]; }
jjmedina	0:a5367bd4e404	1339	aZ_05s[2*Ai] = aZ;
jjmedina	0:a5367bd4e404	1340	for(unsigned int j = 0; j<(Ai+1); j++) { jerk_025s[j] = jerk_025s[j+1]; }
jjmedina	0:a5367bd4e404	1341	jerk_025s[Ai] = RegresionJerk(aZ_05s, 2*Ai+1);
jjmedina	0:a5367bd4e404	1342
jjmedina	0:a5367bd4e404	1343	//Ojo, el jerk esta desfasado 0.25 segundos. Claculo su m�xdimo en la siguiente pasada.
jjmedina	0:a5367bd4e404	1344
jjmedina	0:a5367bd4e404	1345	//calculo Vmax a partir de Vmax1 y Vmax2.
jjmedina	0:a5367bd4e404	1346	if (Vmax1<Vmax2)
jjmedina	0:a5367bd4e404	1347	{
jjmedina	0:a5367bd4e404	1348	Vmax = Vmax2;
jjmedina	0:a5367bd4e404	1349	}else
jjmedina	0:a5367bd4e404	1350	{
jjmedina	0:a5367bd4e404	1351	Vmax = Vmax1;
jjmedina	0:a5367bd4e404	1352	}
jjmedina	0:a5367bd4e404	1353
jjmedina	0:a5367bd4e404	1354	if(i== nPar) {d_par = dZ;}
jjmedina	0:a5367bd4e404	1355
jjmedina	0:a5367bd4e404	1356
jjmedina	0:a5367bd4e404	1357	fprintf(fp2,"%f %f %f %f %f \n", t, dZ, vZ, aZ, jerk_025s[0]);
jjmedina	0:a5367bd4e404	1358	}
jjmedina	0:a5367bd4e404	1359
jjmedina	0:a5367bd4e404	1360
jjmedina	0:a5367bd4e404	1361
jjmedina	0:a5367bd4e404	1362
jjmedina	0:a5367bd4e404	1363	fclose(fp);
jjmedina	0:a5367bd4e404	1364	fclose(fp2);
jjmedina	0:a5367bd4e404	1365
jjmedina	0:a5367bd4e404	1366
jjmedina	0:a5367bd4e404	1367
jjmedina	0:a5367bd4e404	1368	//ELIMINA ARCHIVO dva0
jjmedina	0:a5367bd4e404	1369	int rmDVA0 = remove(dispVelFile); if(rmDVA0 != 0) { display.Clear();display.SetXY(0,0); display.printf("Error removing %s file\n", dispVelFile); }
jjmedina	0:a5367bd4e404	1370
jjmedina	0:a5367bd4e404	1371	}
jjmedina	0:a5367bd4e404	1372
jjmedina	0:a5367bd4e404	1373	void calcula_n05n95_Boveloc_jerkmax(unsigned int ContadorEnsayos, unsigned int ContadorDatos, int PasoTiempo)
jjmedina	0:a5367bd4e404	1374	{
jjmedina	0:a5367bd4e404	1375
jjmedina	0:a5367bd4e404	1376	//float jerkmax = 0.0;//POR PANTALLA
jjmedina	0:a5367bd4e404	1377	float t;
jjmedina	0:a5367bd4e404	1378	float dZ, vZ, aZ, jerk;
jjmedina	0:a5367bd4e404	1379	int CalculaBo2(0);
jjmedina	0:a5367bd4e404	1380
jjmedina	0:a5367bd4e404	1381	//5% and 95% of Vmax1 y Vmax2
jjmedina	0:a5367bd4e404	1382	unsigned int c1(0);unsigned int c2(0);unsigned int c3(0);unsigned int c4(0);
jjmedina	0:a5367bd4e404	1383
jjmedina	0:a5367bd4e404	1384
jjmedina	0:a5367bd4e404	1385	//Velocidad A95
jjmedina	0:a5367bd4e404	1386	const float A95max_veloc_acel = 0.95*Vmax1;
jjmedina	0:a5367bd4e404	1387	const float A95max_veloc_decel = 0.95*Vmax2;
jjmedina	0:a5367bd4e404	1388	unsigned int contA95 = Bo1;
jjmedina	0:a5367bd4e404	1389	unsigned int contA95_2 = Boacel + 1;
jjmedina	0:a5367bd4e404	1390	bool Bo_veloc_acel_done = false;
jjmedina	0:a5367bd4e404	1391	bool Bo_veloc_decel_done = false;
jjmedina	0:a5367bd4e404	1392	float t2(0.0);
jjmedina	0:a5367bd4e404	1393	float t3(0.0);
jjmedina	0:a5367bd4e404	1394
jjmedina	0:a5367bd4e404	1395	//Archivos de lectura y escritura
jjmedina	0:a5367bd4e404	1396
jjmedina	0:a5367bd4e404	1397	FILE * fp;
jjmedina	0:a5367bd4e404	1398
jjmedina	0:a5367bd4e404	1399	char NoViajel[2];
jjmedina	0:a5367bd4e404	1400	char velMod[50];
jjmedina	0:a5367bd4e404	1401
jjmedina	0:a5367bd4e404	1402	strcpy(velMod,"/local/dvaj_");
jjmedina	0:a5367bd4e404	1403
jjmedina	0:a5367bd4e404	1404	sprintf (NoViajel, "%d", ContadorEnsayos);
jjmedina	0:a5367bd4e404	1405	strcat(velMod,NoViajel);
jjmedina	0:a5367bd4e404	1406	strcat(velMod,".txt");
jjmedina	0:a5367bd4e404	1407
jjmedina	0:a5367bd4e404	1408	fp = fopen(velMod,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", velMod); }
jjmedina	0:a5367bd4e404	1409
jjmedina	0:a5367bd4e404	1410	int nDatos = (ContadorDatos-(ndes+nmed));
jjmedina	0:a5367bd4e404	1411	if (nDatos < 0) {nDatos = 0;}
jjmedina	0:a5367bd4e404	1412
jjmedina	0:a5367bd4e404	1413	//loop principal
jjmedina	0:a5367bd4e404	1414	for(unsigned int i = 0; i<nDatos; i++)
jjmedina	0:a5367bd4e404	1415	{
jjmedina	0:a5367bd4e404	1416	//read data row
jjmedina	0:a5367bd4e404	1417	fscanf(fp,"%f %f %f %f %f", &t, &dZ, &vZ, &aZ, &jerk);
jjmedina	0:a5367bd4e404	1418
jjmedina	0:a5367bd4e404	1419	if( i == nV0) { CalculaBo2 = 1; }
jjmedina	0:a5367bd4e404	1420
jjmedina	0:a5367bd4e404	1421	if ( (fabs(fabs(dZ)-fabs(d_par))<0.5) && (i>Bo1) && (CalculaBo2 == 1) )
jjmedina	0:a5367bd4e404	1422	{
jjmedina	0:a5367bd4e404	1423	Bo2 = i;
jjmedina	0:a5367bd4e404	1424	CalculaBo2 = 0;
jjmedina	0:a5367bd4e404	1425	}
jjmedina	0:a5367bd4e404	1426
jjmedina	0:a5367bd4e404	1427	//jerkmax
jjmedina	0:a5367bd4e404	1428	if ( fabs(jerk) >= jerkmax )
jjmedina	0:a5367bd4e404	1429	{
jjmedina	0:a5367bd4e404	1430	jerkmax = fabs(jerk);
jjmedina	0:a5367bd4e404	1431	}
jjmedina	0:a5367bd4e404	1432
jjmedina	0:a5367bd4e404	1433	//5% and 95% of Vmax1
jjmedina	0:a5367bd4e404	1434	if ( (i >= Bo0) && (i <= Boacel) )
jjmedina	0:a5367bd4e404	1435	{
jjmedina	0:a5367bd4e404	1436	if( ( fabs(vZ) > 0.05*Vmax1 ) && (c1 == 0) )
jjmedina	0:a5367bd4e404	1437	{
jjmedina	0:a5367bd4e404	1438	n05Vmax1 = i;
jjmedina	0:a5367bd4e404	1439	c1 = 1;
jjmedina	0:a5367bd4e404	1440	}
jjmedina	0:a5367bd4e404	1441	if( ( fabs(vZ) > 0.95*Vmax1 ) && (c2 == 0) )
jjmedina	0:a5367bd4e404	1442	{
jjmedina	0:a5367bd4e404	1443	n95Vmax1 = i;
jjmedina	0:a5367bd4e404	1444	c2 = 1;
jjmedina	0:a5367bd4e404	1445	}
jjmedina	0:a5367bd4e404	1446	}
jjmedina	0:a5367bd4e404	1447
jjmedina	0:a5367bd4e404	1448	//5% and 95% of Vmax2
jjmedina	0:a5367bd4e404	1449	if ( (i >= (Boacel + 1)) && (i < Bo3) )
jjmedina	0:a5367bd4e404	1450	{
jjmedina	0:a5367bd4e404	1451	if( ( fabs(vZ) < 0.95*Vmax2 ) && (c3 == 0) )
jjmedina	0:a5367bd4e404	1452	{
jjmedina	0:a5367bd4e404	1453	n95Vmax2 = i;
jjmedina	0:a5367bd4e404	1454	c3 = 1;
jjmedina	0:a5367bd4e404	1455	}
jjmedina	0:a5367bd4e404	1456	if( ( fabs(vZ) < 0.05*Vmax2 ) && (c4 == 0) )
jjmedina	0:a5367bd4e404	1457	{
jjmedina	0:a5367bd4e404	1458	n05Vmax2 = i;
jjmedina	0:a5367bd4e404	1459	c4 = 1;
jjmedina	0:a5367bd4e404	1460	}
jjmedina	0:a5367bd4e404	1461	}
jjmedina	0:a5367bd4e404	1462
jjmedina	0:a5367bd4e404	1463	//Bo_veloc_acel y Bo_veloc_decel
jjmedina	0:a5367bd4e404	1464	if( ( (i > Bo1) && (i<=Boacel) ) && ( Bo_veloc_acel_done == false ) )
jjmedina	0:a5367bd4e404	1465	{
jjmedina	0:a5367bd4e404	1466	if ( fabs(vZ) >= A95max_veloc_acel )
jjmedina	0:a5367bd4e404	1467	{
jjmedina	0:a5367bd4e404	1468	Bo_veloc_acel = contA95;
jjmedina	0:a5367bd4e404	1469	t2 = t + 1.0;
jjmedina	0:a5367bd4e404	1470	Bo_veloc_acel_done = true;
jjmedina	0:a5367bd4e404	1471	}
jjmedina	0:a5367bd4e404	1472	contA95++;
jjmedina	0:a5367bd4e404	1473	}
jjmedina	0:a5367bd4e404	1474	if( ( (i > (Boacel + 1)) && (i<=Bo3)) && (Bo_veloc_decel_done == false) )
jjmedina	0:a5367bd4e404	1475	{
jjmedina	0:a5367bd4e404	1476	if ( fabs(vZ) >= A95max_veloc_decel )
jjmedina	0:a5367bd4e404	1477	{
jjmedina	0:a5367bd4e404	1478	Bo_veloc_decel = contA95_2;
jjmedina	0:a5367bd4e404	1479	t3 = t - 1.0;
jjmedina	0:a5367bd4e404	1480	Bo_veloc_decel_done = true;
jjmedina	0:a5367bd4e404	1481	}
jjmedina	0:a5367bd4e404	1482	contA95_2++;
jjmedina	0:a5367bd4e404	1483	}
jjmedina	0:a5367bd4e404	1484
jjmedina	0:a5367bd4e404	1485
jjmedina	0:a5367bd4e404	1486	}//loop principal
jjmedina	0:a5367bd4e404	1487
jjmedina	0:a5367bd4e404	1488	//calculos after loop
jjmedina	0:a5367bd4e404	1489	//t2 y t3
jjmedina	0:a5367bd4e404	1490
jjmedina	0:a5367bd4e404	1491	for (unsigned int j = 0 ; j<=100 ; j++)
jjmedina	0:a5367bd4e404	1492	{
jjmedina	0:a5367bd4e404	1493	if(t3<t2)
jjmedina	0:a5367bd4e404	1494	{
jjmedina	0:a5367bd4e404	1495	t2 = t2 - ((float)(1))/10.0;
jjmedina	0:a5367bd4e404	1496	t3 = t3 + ((float)(1))/10.0;
jjmedina	0:a5367bd4e404	1497	}else
jjmedina	0:a5367bd4e404	1498	{
jjmedina	0:a5367bd4e404	1499	Bo_veloc_acel = (unsigned int)( Bo_veloc_acel + (1.0/((float)(PasoTiempo/1000000.0))) - ((float)(j))/(10.0*((float)(PasoTiempo/1000000.0))) );
jjmedina	0:a5367bd4e404	1500	Bo_veloc_decel = (unsigned int)( Bo_veloc_decel - (1.0/((float)(PasoTiempo/1000000.0))) + ((float)(j))/(10.0*((float)(PasoTiempo/1000000.0))) );
jjmedina	0:a5367bd4e404	1501	break;
jjmedina	0:a5367bd4e404	1502	}
jjmedina	0:a5367bd4e404	1503	}
jjmedina	0:a5367bd4e404	1504
jjmedina	0:a5367bd4e404	1505
jjmedina	0:a5367bd4e404	1506
jjmedina	0:a5367bd4e404	1507
jjmedina	0:a5367bd4e404	1508	//seguridad
jjmedina	0:a5367bd4e404	1509	//seguridad por si nVmax1
jjmedina	0:a5367bd4e404	1510	if ( c1 == 0 )
jjmedina	0:a5367bd4e404	1511	{
jjmedina	0:a5367bd4e404	1512	n05Vmax1 = (unsigned int)(Bo0 + (Bo1-Bo0)/2.0);
jjmedina	0:a5367bd4e404	1513	}
jjmedina	0:a5367bd4e404	1514	if ( c2 == 0 )
jjmedina	0:a5367bd4e404	1515	{
jjmedina	0:a5367bd4e404	1516	n95Vmax1 = Boacel;
jjmedina	0:a5367bd4e404	1517	}
jjmedina	0:a5367bd4e404	1518	//seguridad por si nVmax2
jjmedina	0:a5367bd4e404	1519	if ( c3 == 0 )
jjmedina	0:a5367bd4e404	1520	{
jjmedina	0:a5367bd4e404	1521	n95Vmax2 = Boacel;
jjmedina	0:a5367bd4e404	1522	}
jjmedina	0:a5367bd4e404	1523	if ( c4 == 0 )
jjmedina	0:a5367bd4e404	1524	{
jjmedina	0:a5367bd4e404	1525	n05Vmax2 = (unsigned int)(Bo2 + (Bo3-Bo2)/2.0);
jjmedina	0:a5367bd4e404	1526	}
jjmedina	0:a5367bd4e404	1527	//cierra ficheros
jjmedina	0:a5367bd4e404	1528	fclose(fp);
jjmedina	0:a5367bd4e404	1529
jjmedina	0:a5367bd4e404	1530
jjmedina	0:a5367bd4e404	1531	}
jjmedina	0:a5367bd4e404	1532
jjmedina	0:a5367bd4e404	1533
jjmedina	0:a5367bd4e404	1534
jjmedina	0:a5367bd4e404	1535	void calcula_A95_V_acel_decel(unsigned int ContadorEnsayos, unsigned int ContadorDatos, int PasoTiempo)
jjmedina	0:a5367bd4e404	1536
jjmedina	0:a5367bd4e404	1537	{
jjmedina	0:a5367bd4e404	1538	float t;
jjmedina	0:a5367bd4e404	1539	float dZ, vZ, aZ, jerk;
jjmedina	0:a5367bd4e404	1540
jjmedina	0:a5367bd4e404	1541	//VA95
jjmedina	0:a5367bd4e404	1542	int length_maximoVA95(0);
jjmedina	0:a5367bd4e404	1543	if( Bo_veloc_decel >= Bo_veloc_acel )
jjmedina	0:a5367bd4e404	1544	{
jjmedina	0:a5367bd4e404	1545	length_maximoVA95 = (int)((Bo_veloc_decel - Bo_veloc_acel)*0.05) + 1;
jjmedina	0:a5367bd4e404	1546	}else
jjmedina	0:a5367bd4e404	1547	{
jjmedina	0:a5367bd4e404	1548	length_maximoVA95 = 1;
jjmedina	0:a5367bd4e404	1549	}
jjmedina	0:a5367bd4e404	1550	//int length_maximoVA95 = (int)((Bo_veloc_decel - Bo_veloc_acel)*0.05) + 1;
jjmedina	0:a5367bd4e404	1551
jjmedina	0:a5367bd4e404	1552	int maxindex(0);
jjmedina	0:a5367bd4e404	1553	vector<float> *maximoAV95 = new vector<float> (length_maximoVA95,0.0);
jjmedina	0:a5367bd4e404	1554	//vector<float> maximoAV95 (length_maximoVA95,0.0);
jjmedina	0:a5367bd4e404	1555	vector<float> *diferencia_vec = new vector<float>;
jjmedina	0:a5367bd4e404	1556	vector<float>::iterator up;
jjmedina	0:a5367bd4e404	1557	float diferencia;
jjmedina	0:a5367bd4e404	1558	float maximo;
jjmedina	0:a5367bd4e404	1559	int contadorA95(0);
jjmedina	0:a5367bd4e404	1560	//aA95
jjmedina	0:a5367bd4e404	1561	int length_maximo_aA95(0);
jjmedina	0:a5367bd4e404	1562	if( n95Vmax1 >= n05Vmax1 )
jjmedina	0:a5367bd4e404	1563	{
jjmedina	0:a5367bd4e404	1564	length_maximo_aA95 = (int)((n95Vmax1 - n05Vmax1)*0.05) + 1;
jjmedina	0:a5367bd4e404	1565	}else
jjmedina	0:a5367bd4e404	1566	{
jjmedina	0:a5367bd4e404	1567	length_maximo_aA95 = 1;
jjmedina	0:a5367bd4e404	1568	}
jjmedina	0:a5367bd4e404	1569	//int length_maximo_aA95 = (int)((n95Vmax1 - n05Vmax1)*0.05) + 1;
jjmedina	0:a5367bd4e404	1570
jjmedina	0:a5367bd4e404	1571	int maxindex_aA95(0);
jjmedina	0:a5367bd4e404	1572	vector<float> *maximo_aA95 = new vector<float> (length_maximo_aA95,0.0);
jjmedina	0:a5367bd4e404	1573	//vector<float> maximo_aA95 (length_maximo_aA95,0.0);
jjmedina	0:a5367bd4e404	1574	vector<float> *diferencia_aA95_vec = new vector<float>;
jjmedina	0:a5367bd4e404	1575	vector<float>::iterator up_aA95;
jjmedina	0:a5367bd4e404	1576	float diferencia_aA95;
jjmedina	0:a5367bd4e404	1577	float maximoaA95;
jjmedina	0:a5367bd4e404	1578	int contador_aA95(0);
jjmedina	0:a5367bd4e404	1579	//dA95
jjmedina	0:a5367bd4e404	1580	int length_maximo_dA95(0);
jjmedina	0:a5367bd4e404	1581	if( n05Vmax2 >= n95Vmax2 )
jjmedina	0:a5367bd4e404	1582	{
jjmedina	0:a5367bd4e404	1583	length_maximo_dA95 = (int)((n05Vmax2 - n95Vmax2)*0.05) + 1;
jjmedina	0:a5367bd4e404	1584	}else
jjmedina	0:a5367bd4e404	1585	{
jjmedina	0:a5367bd4e404	1586	length_maximo_dA95 = 1;
jjmedina	0:a5367bd4e404	1587	}
jjmedina	0:a5367bd4e404	1588	//int length_maximo_dA95 = (int)((n05Vmax2 - n95Vmax2)*0.05) + 1;
jjmedina	0:a5367bd4e404	1589
jjmedina	0:a5367bd4e404	1590	int maxindex_dA95(0);
jjmedina	0:a5367bd4e404	1591	vector<float> *maximo_dA95 = new vector<float> (length_maximo_dA95,0.0);
jjmedina	0:a5367bd4e404	1592	//vector<float> maximo_dA95 (length_maximo_dA95,0.0);
jjmedina	0:a5367bd4e404	1593	vector<float> *diferencia_dA95_vec = new vector<float>;
jjmedina	0:a5367bd4e404	1594	vector<float>::iterator up_dA95;
jjmedina	0:a5367bd4e404	1595	float diferencia_dA95;
jjmedina	0:a5367bd4e404	1596	float maximodA95;
jjmedina	0:a5367bd4e404	1597	int contador_dA95(0);
jjmedina	0:a5367bd4e404	1598
jjmedina	0:a5367bd4e404	1599	//Archivos de lectura y escritura
jjmedina	0:a5367bd4e404	1600
jjmedina	0:a5367bd4e404	1601	FILE * fp;
jjmedina	0:a5367bd4e404	1602
jjmedina	0:a5367bd4e404	1603	char NoViajel[2];
jjmedina	0:a5367bd4e404	1604	char data[50];
jjmedina	0:a5367bd4e404	1605
jjmedina	0:a5367bd4e404	1606	strcpy(data,"/local/dvaj_");
jjmedina	0:a5367bd4e404	1607	sprintf (NoViajel, "%d", ContadorEnsayos);
jjmedina	0:a5367bd4e404	1608	strcat(data,NoViajel);
jjmedina	0:a5367bd4e404	1609	strcat(data,".txt");
jjmedina	0:a5367bd4e404	1610
jjmedina	0:a5367bd4e404	1611	fp = fopen(data,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", data); }
jjmedina	0:a5367bd4e404	1612
jjmedina	0:a5367bd4e404	1613	int nDatos = (ContadorDatos-(ndes+nmed));
jjmedina	0:a5367bd4e404	1614	if (nDatos < 0) {nDatos = 0;}
jjmedina	0:a5367bd4e404	1615
jjmedina	0:a5367bd4e404	1616
jjmedina	0:a5367bd4e404	1617	if ( (maximoAV95 == NULL) \|\| ( maximo_aA95 == NULL) \|\| ( maximo_dA95 == NULL) )
jjmedina	0:a5367bd4e404	1618	{
jjmedina	0:a5367bd4e404	1619	display.Clear();display.SetXY(0,0); display.printf("RAM overflow\n");
jjmedina	0:a5367bd4e404	1620	}else
jjmedina	0:a5367bd4e404	1621	{
jjmedina	0:a5367bd4e404	1622	//loop principal, pasada 1
jjmedina	0:a5367bd4e404	1623	for(unsigned int i = 0; i<(nDatos); i++)
jjmedina	0:a5367bd4e404	1624	{
jjmedina	0:a5367bd4e404	1625	//read data row
jjmedina	0:a5367bd4e404	1626	fscanf(fp,"%f %f %f %f %f", &t, &dZ, &vZ, &aZ, &jerk);
jjmedina	0:a5367bd4e404	1627
jjmedina	0:a5367bd4e404	1628	//VA95
jjmedina	0:a5367bd4e404	1629	if( (i <= Bo_veloc_decel) && ( i >= Bo_veloc_acel) )
jjmedina	0:a5367bd4e404	1630	{
jjmedina	0:a5367bd4e404	1631
jjmedina	0:a5367bd4e404	1632	//pasada 1
jjmedina	0:a5367bd4e404	1633	for (unsigned int j = 0; j<length_maximoVA95; j++)
jjmedina	0:a5367bd4e404	1634	{
jjmedina	0:a5367bd4e404	1635
jjmedina	0:a5367bd4e404	1636	diferencia = fabs(vZ) - maximoAV95->at(j);
jjmedina	0:a5367bd4e404	1637	if ( diferencia < 0 )
jjmedina	0:a5367bd4e404	1638	{
jjmedina	0:a5367bd4e404	1639	diferencia_vec->push_back(0.0);
jjmedina	0:a5367bd4e404	1640	contadorA95++;
jjmedina	0:a5367bd4e404	1641	}else
jjmedina	0:a5367bd4e404	1642	{
jjmedina	0:a5367bd4e404	1643	diferencia_vec->push_back(diferencia);
jjmedina	0:a5367bd4e404	1644	}
jjmedina	0:a5367bd4e404	1645
jjmedina	0:a5367bd4e404	1646	}
jjmedina	0:a5367bd4e404	1647	if ( contadorA95 == length_maximoVA95 ){}
jjmedina	0:a5367bd4e404	1648	else
jjmedina	0:a5367bd4e404	1649	{
jjmedina	0:a5367bd4e404	1650
jjmedina	0:a5367bd4e404	1651	maximo = *max_element(diferencia_vec->begin(),diferencia_vec->end());
jjmedina	0:a5367bd4e404	1652	up = find(diferencia_vec->begin(), diferencia_vec->end(), maximo);
jjmedina	0:a5367bd4e404	1653	maxindex = int(up - diferencia_vec->begin());
jjmedina	0:a5367bd4e404	1654	maximoAV95->erase(maximoAV95->begin() + maxindex );
jjmedina	0:a5367bd4e404	1655	maximoAV95->insert(maximoAV95->begin() + maxindex ,fabs(vZ));
jjmedina	0:a5367bd4e404	1656	}
jjmedina	0:a5367bd4e404	1657	contadorA95 = 0;
jjmedina	0:a5367bd4e404	1658	diferencia_vec->clear();
jjmedina	0:a5367bd4e404	1659	}
jjmedina	0:a5367bd4e404	1660
jjmedina	0:a5367bd4e404	1661	//aA95
jjmedina	0:a5367bd4e404	1662	if( (i <= n95Vmax1) && ( i >= n05Vmax1) )
jjmedina	0:a5367bd4e404	1663	{
jjmedina	0:a5367bd4e404	1664
jjmedina	0:a5367bd4e404	1665	//pasada 1
jjmedina	0:a5367bd4e404	1666	for (unsigned int j = 0; j<length_maximo_aA95; j++)
jjmedina	0:a5367bd4e404	1667	{
jjmedina	0:a5367bd4e404	1668
jjmedina	0:a5367bd4e404	1669	diferencia_aA95 = fabs(aZ) - maximo_aA95->at(j);
jjmedina	0:a5367bd4e404	1670	if ( diferencia_aA95 < 0 )
jjmedina	0:a5367bd4e404	1671	{
jjmedina	0:a5367bd4e404	1672	diferencia_aA95_vec->push_back(0.0);
jjmedina	0:a5367bd4e404	1673	contador_aA95++;
jjmedina	0:a5367bd4e404	1674	}else
jjmedina	0:a5367bd4e404	1675	{
jjmedina	0:a5367bd4e404	1676	diferencia_aA95_vec->push_back(diferencia_aA95);
jjmedina	0:a5367bd4e404	1677	}
jjmedina	0:a5367bd4e404	1678
jjmedina	0:a5367bd4e404	1679	}
jjmedina	0:a5367bd4e404	1680	if ( contador_aA95 == length_maximo_aA95 ){}
jjmedina	0:a5367bd4e404	1681	else
jjmedina	0:a5367bd4e404	1682	{
jjmedina	0:a5367bd4e404	1683
jjmedina	0:a5367bd4e404	1684	maximoaA95 = *max_element(diferencia_aA95_vec->begin(),diferencia_aA95_vec->end());
jjmedina	0:a5367bd4e404	1685	up_aA95 = find(diferencia_aA95_vec->begin(), diferencia_aA95_vec->end(), maximoaA95);
jjmedina	0:a5367bd4e404	1686	maxindex_aA95 = int(up_aA95 - diferencia_aA95_vec->begin());
jjmedina	0:a5367bd4e404	1687	maximo_aA95->erase(maximo_aA95->begin() + maxindex_aA95 );
jjmedina	0:a5367bd4e404	1688	maximo_aA95->insert(maximo_aA95->begin() + maxindex_aA95 ,fabs(aZ));
jjmedina	0:a5367bd4e404	1689	}
jjmedina	0:a5367bd4e404	1690	contador_aA95 = 0;
jjmedina	0:a5367bd4e404	1691	diferencia_aA95_vec->clear();
jjmedina	0:a5367bd4e404	1692	}
jjmedina	0:a5367bd4e404	1693
jjmedina	0:a5367bd4e404	1694	//dA95
jjmedina	0:a5367bd4e404	1695	if( (i <= n05Vmax2) && ( i >= n95Vmax2) )
jjmedina	0:a5367bd4e404	1696	{
jjmedina	0:a5367bd4e404	1697
jjmedina	0:a5367bd4e404	1698	//pasada 1
jjmedina	0:a5367bd4e404	1699	for (unsigned int j = 0; j<length_maximo_dA95; j++)
jjmedina	0:a5367bd4e404	1700	{
jjmedina	0:a5367bd4e404	1701
jjmedina	0:a5367bd4e404	1702	diferencia_dA95 = fabs(aZ) - maximo_dA95->at(j);
jjmedina	0:a5367bd4e404	1703	if ( diferencia_dA95 < 0 )
jjmedina	0:a5367bd4e404	1704	{
jjmedina	0:a5367bd4e404	1705	diferencia_dA95_vec->push_back(0.0);
jjmedina	0:a5367bd4e404	1706	contador_dA95++;
jjmedina	0:a5367bd4e404	1707	}else
jjmedina	0:a5367bd4e404	1708	{
jjmedina	0:a5367bd4e404	1709	diferencia_dA95_vec->push_back(diferencia_dA95);
jjmedina	0:a5367bd4e404	1710	}
jjmedina	0:a5367bd4e404	1711
jjmedina	0:a5367bd4e404	1712	}
jjmedina	0:a5367bd4e404	1713	if ( contador_dA95 == length_maximo_dA95 ){}
jjmedina	0:a5367bd4e404	1714	else
jjmedina	0:a5367bd4e404	1715	{
jjmedina	0:a5367bd4e404	1716
jjmedina	0:a5367bd4e404	1717	maximodA95 = *max_element(diferencia_dA95_vec->begin(),diferencia_dA95_vec->end());
jjmedina	0:a5367bd4e404	1718	up_dA95 = find(diferencia_dA95_vec->begin(), diferencia_dA95_vec->end(), maximodA95);
jjmedina	0:a5367bd4e404	1719	maxindex_dA95 = int(up_dA95 - diferencia_dA95_vec->begin());
jjmedina	0:a5367bd4e404	1720	maximo_dA95->erase(maximo_dA95->begin() + maxindex_dA95 );
jjmedina	0:a5367bd4e404	1721	maximo_dA95->insert(maximo_dA95->begin() + maxindex_dA95 ,fabs(aZ));
jjmedina	0:a5367bd4e404	1722	}
jjmedina	0:a5367bd4e404	1723	contador_dA95 = 0;
jjmedina	0:a5367bd4e404	1724	diferencia_dA95_vec->clear();
jjmedina	0:a5367bd4e404	1725	}
jjmedina	0:a5367bd4e404	1726
jjmedina	0:a5367bd4e404	1727	}//Fin loop principal, pasada 1.
jjmedina	0:a5367bd4e404	1728
jjmedina	0:a5367bd4e404	1729	sort(maximoAV95->begin(),maximoAV95->end());//ascending
jjmedina	0:a5367bd4e404	1730	reverse(maximoAV95->begin(),maximoAV95->end());//descending
jjmedina	0:a5367bd4e404	1731	sort(maximo_aA95->begin(),maximo_aA95->end());//ascending
jjmedina	0:a5367bd4e404	1732	reverse(maximo_aA95->begin(),maximo_aA95->end());//descending
jjmedina	0:a5367bd4e404	1733	sort(maximo_dA95->begin(),maximo_dA95->end());//ascending
jjmedina	0:a5367bd4e404	1734	reverse(maximo_dA95->begin(),maximo_dA95->end());//descending
jjmedina	0:a5367bd4e404	1735
jjmedina	0:a5367bd4e404	1736
jjmedina	0:a5367bd4e404	1737	//cierra ficheros y variables
jjmedina	0:a5367bd4e404	1738	fclose(fp);
jjmedina	0:a5367bd4e404	1739
jjmedina	0:a5367bd4e404	1740	//pasada2
jjmedina	0:a5367bd4e404	1741	fp = fopen(data,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", data); }
jjmedina	0:a5367bd4e404	1742
jjmedina	0:a5367bd4e404	1743
jjmedina	0:a5367bd4e404	1744	//loop principal, pasada 2
jjmedina	0:a5367bd4e404	1745	double resta(1000.0);
jjmedina	0:a5367bd4e404	1746	double resta_aA95(1000.0);
jjmedina	0:a5367bd4e404	1747	double resta_dA95(1000.0);
jjmedina	0:a5367bd4e404	1748	for(unsigned int i = 0; i<(nDatos); i++)
jjmedina	0:a5367bd4e404	1749	{
jjmedina	0:a5367bd4e404	1750	//read data row
jjmedina	0:a5367bd4e404	1751	fscanf(fp,"%f %f %f %f %f", &t, &dZ, &vZ, &aZ, &jerk);
jjmedina	0:a5367bd4e404	1752
jjmedina	0:a5367bd4e404	1753	//VA95
jjmedina	0:a5367bd4e404	1754
jjmedina	0:a5367bd4e404	1755	if( (i <= Bo_veloc_decel) && ( i >= Bo_veloc_acel) )
jjmedina	0:a5367bd4e404	1756	{
jjmedina	0:a5367bd4e404	1757	if( ((maximoAV95->at(maximoAV95->size() - 1) - fabs(vZ)) <= resta) && (fabs(vZ) < maximoAV95->at(maximoAV95->size() - 1)) )
jjmedina	0:a5367bd4e404	1758	{
jjmedina	0:a5367bd4e404	1759	VA95 = fabs(vZ);
jjmedina	0:a5367bd4e404	1760	resta = maximoAV95->at(maximoAV95->size() - 1) - fabs(vZ);
jjmedina	0:a5367bd4e404	1761	}
jjmedina	0:a5367bd4e404	1762	}
jjmedina	0:a5367bd4e404	1763	//aA95
jjmedina	0:a5367bd4e404	1764
jjmedina	0:a5367bd4e404	1765	if( (i <= n95Vmax1) && ( i >= n05Vmax1) )
jjmedina	0:a5367bd4e404	1766	{
jjmedina	0:a5367bd4e404	1767	if( ((maximo_aA95->at(maximo_aA95->size() - 1) - fabs(aZ)) <= resta_aA95) && (fabs(aZ) < maximo_aA95->at(maximo_aA95->size() - 1)) )
jjmedina	0:a5367bd4e404	1768	{
jjmedina	0:a5367bd4e404	1769	aA95 = fabs(aZ);
jjmedina	0:a5367bd4e404	1770	resta_aA95 = maximo_aA95->at(maximo_aA95->size() - 1) - fabs(aZ);
jjmedina	0:a5367bd4e404	1771	}
jjmedina	0:a5367bd4e404	1772	}
jjmedina	0:a5367bd4e404	1773	//dA95
jjmedina	0:a5367bd4e404	1774
jjmedina	0:a5367bd4e404	1775
jjmedina	0:a5367bd4e404	1776
jjmedina	0:a5367bd4e404	1777	if( (i <= n05Vmax2) && ( i >= n95Vmax2) )
jjmedina	0:a5367bd4e404	1778	{
jjmedina	0:a5367bd4e404	1779	if( ((maximo_dA95->at(maximo_dA95->size() - 1) - fabs(aZ)) <= resta_dA95) && (fabs(aZ) < maximo_dA95->at(maximo_dA95->size() - 1)) )
jjmedina	0:a5367bd4e404	1780	{
jjmedina	0:a5367bd4e404	1781	dA95 = fabs(aZ);
jjmedina	0:a5367bd4e404	1782	resta_dA95 = maximo_dA95->at(maximo_dA95->size() - 1) - fabs(aZ);
jjmedina	0:a5367bd4e404	1783	}
jjmedina	0:a5367bd4e404	1784	}
jjmedina	0:a5367bd4e404	1785
jjmedina	0:a5367bd4e404	1786
jjmedina	0:a5367bd4e404	1787
jjmedina	0:a5367bd4e404	1788	}
jjmedina	0:a5367bd4e404	1789	//clear all vectors
jjmedina	0:a5367bd4e404	1790	maximoAV95->clear();
jjmedina	0:a5367bd4e404	1791	delete maximoAV95;
jjmedina	0:a5367bd4e404	1792	diferencia_vec->clear();
jjmedina	0:a5367bd4e404	1793	delete diferencia_vec;
jjmedina	0:a5367bd4e404	1794	maximo_aA95->clear();
jjmedina	0:a5367bd4e404	1795	delete maximo_aA95;
jjmedina	0:a5367bd4e404	1796	diferencia_aA95_vec->clear();
jjmedina	0:a5367bd4e404	1797	delete diferencia_aA95_vec;
jjmedina	0:a5367bd4e404	1798	maximo_dA95->clear();
jjmedina	0:a5367bd4e404	1799	delete maximo_dA95;
jjmedina	0:a5367bd4e404	1800	diferencia_dA95_vec->clear();
jjmedina	0:a5367bd4e404	1801	delete diferencia_dA95_vec;
jjmedina	0:a5367bd4e404	1802
jjmedina	0:a5367bd4e404	1803	}//fin else
jjmedina	0:a5367bd4e404	1804
jjmedina	0:a5367bd4e404	1805	fclose(fp);
jjmedina	0:a5367bd4e404	1806
jjmedina	0:a5367bd4e404	1807	}
jjmedina	0:a5367bd4e404	1808
jjmedina	0:a5367bd4e404	1809	void vibraReadFilter(unsigned int ContadorEnsayos, unsigned int ContadorDatos)
jjmedina	0:a5367bd4e404	1810	{
jjmedina	0:a5367bd4e404	1811
jjmedina	0:a5367bd4e404	1812
jjmedina	0:a5367bd4e404	1813	float t;
jjmedina	0:a5367bd4e404	1814	float aXYZ[3]; //vector aceleracion en el paso i [aX,aY,aZ]
jjmedina	0:a5367bd4e404	1815	float Zvibweighted[3]={0.0,0.0,0.0};
jjmedina	0:a5367bd4e404	1816	float Xvibweighted[3]={0.0,0.0,0.0};
jjmedina	0:a5367bd4e404	1817	float Yvibweighted[3]={0.0,0.0,0.0};
jjmedina	0:a5367bd4e404	1818	float aZ[3], aX[3], aY[3];
jjmedina	0:a5367bd4e404	1819
jjmedina	0:a5367bd4e404	1820	FILE * fp;
jjmedina	0:a5367bd4e404	1821	FILE * fp3;
jjmedina	0:a5367bd4e404	1822
jjmedina	0:a5367bd4e404	1823	char NoViajel[2];
jjmedina	0:a5367bd4e404	1824	char resultsFile[50];
jjmedina	0:a5367bd4e404	1825	char resultsFileW[50];
jjmedina	0:a5367bd4e404	1826
jjmedina	0:a5367bd4e404	1827	//strcpy(resultsFile,"/local/IV_");
jjmedina	0:a5367bd4e404	1828	strcpy(resultsFile,"/local/raw_V_");
jjmedina	0:a5367bd4e404	1829
jjmedina	0:a5367bd4e404	1830	strcpy(resultsFileW,"/local/W_IV_");
jjmedina	0:a5367bd4e404	1831
jjmedina	0:a5367bd4e404	1832	sprintf (NoViajel, "%d", ContadorEnsayos);
jjmedina	0:a5367bd4e404	1833	strcat(resultsFile,NoViajel); strcat(resultsFileW,NoViajel);//fichero con todos los datos de la función anterior IV_
jjmedina	0:a5367bd4e404	1834	strcat(resultsFile,".txt"); strcat(resultsFileW,".txt");
jjmedina	0:a5367bd4e404	1835
jjmedina	0:a5367bd4e404	1836	//fichero con todos los datos de la función anterior IV_
jjmedina	0:a5367bd4e404	1837	fp = fopen(resultsFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", resultsFile); }
jjmedina	0:a5367bd4e404	1838	//fichero donde guardaré
jjmedina	0:a5367bd4e404	1839	fp3 = fopen(resultsFileW,"w"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", resultsFileW); }
jjmedina	0:a5367bd4e404	1840
jjmedina	0:a5367bd4e404	1841	//varias pasadas para isofiltrar ax ay y az. isofilter son 4 filtros, pues 4 pasadas
jjmedina	0:a5367bd4e404	1842
jjmedina	0:a5367bd4e404	1843	//Hemos recordado
jjmedina	0:a5367bd4e404	1844
jjmedina	0:a5367bd4e404	1845	//loop principal. Pasada 1. Hl (Low Pass 100Hz Orden 2)
jjmedina	0:a5367bd4e404	1846	for(unsigned int i = 0; i<(ContadorDatos); i++)
jjmedina	0:a5367bd4e404	1847	{
jjmedina	0:a5367bd4e404	1848	fscanf(fp,"%f %f %f %f", &t, &aXYZ[0], &aXYZ[1], &aXYZ[2]);//lee tiempo y 3 aceleraciones X, Y, Z
jjmedina	0:a5367bd4e404	1849	//restamos gravedad a aZ
jjmedina	0:a5367bd4e404	1850	aXYZ[2] = aXYZ[2] - aZm0;
jjmedina	0:a5367bd4e404	1851	aXYZ[0] = aXYZ[0] - aXm0;
jjmedina	0:a5367bd4e404	1852	aXYZ[1] = aXYZ[1] - aYm0;
jjmedina	0:a5367bd4e404	1853	//isofilter
jjmedina	0:a5367bd4e404	1854
jjmedina	0:a5367bd4e404	1855	int i2 = i-(ndes+nmed); //Nuevo idice para calcular posicion y velocidad
jjmedina	0:a5367bd4e404	1856
jjmedina	0:a5367bd4e404	1857
jjmedina	0:a5367bd4e404	1858	if ( i==0 )
jjmedina	0:a5367bd4e404	1859	{
jjmedina	0:a5367bd4e404	1860	Zvibweighted[0] = aXYZ[2];
jjmedina	0:a5367bd4e404	1861	Xvibweighted[0] = aXYZ[0];
jjmedina	0:a5367bd4e404	1862	Yvibweighted[0] = aXYZ[1];
jjmedina	0:a5367bd4e404	1863	aZ[0] =aXYZ[2];
jjmedina	0:a5367bd4e404	1864	aX[0] =aXYZ[0];
jjmedina	0:a5367bd4e404	1865	aY[0] =aXYZ[1];
jjmedina	0:a5367bd4e404	1866
jjmedina	0:a5367bd4e404	1867
jjmedina	0:a5367bd4e404	1868	// fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[0], Yvibweighted[0], Zvibweighted[0]);
jjmedina	0:a5367bd4e404	1869
jjmedina	0:a5367bd4e404	1870	}else if ( i==1 )
jjmedina	0:a5367bd4e404	1871	{
jjmedina	0:a5367bd4e404	1872	Zvibweighted[1] = aXYZ[2];
jjmedina	0:a5367bd4e404	1873	Xvibweighted[1] = aXYZ[0];
jjmedina	0:a5367bd4e404	1874	Yvibweighted[1] = aXYZ[1];
jjmedina	0:a5367bd4e404	1875	aZ[1] =aXYZ[2];
jjmedina	0:a5367bd4e404	1876	aX[1] =aXYZ[0];
jjmedina	0:a5367bd4e404	1877	aY[1] =aXYZ[1];
jjmedina	0:a5367bd4e404	1878
jjmedina	0:a5367bd4e404	1879	// fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[1], Yvibweighted[1], Zvibweighted[1]);
jjmedina	0:a5367bd4e404	1880
jjmedina	0:a5367bd4e404	1881	}else
jjmedina	0:a5367bd4e404	1882	{
jjmedina	0:a5367bd4e404	1883
jjmedina	0:a5367bd4e404	1884	aZ[2] = aXYZ[2];
jjmedina	0:a5367bd4e404	1885	aX[2] = aXYZ[0];
jjmedina	0:a5367bd4e404	1886	aY[2] = aXYZ[1];
jjmedina	0:a5367bd4e404	1887
jjmedina	0:a5367bd4e404	1888	Zvibweighted[2] = isoFilter_Wk_z_Hl(Zvibweighted[0],Zvibweighted[1],aZ, PasoTiempo);
jjmedina	0:a5367bd4e404	1889	Xvibweighted[2] = isoFilter_Wd_xy_Hl(Xvibweighted[0],Xvibweighted[1],aX, PasoTiempo);
jjmedina	0:a5367bd4e404	1890	Yvibweighted[2] = isoFilter_Wd_xy_Hl(Yvibweighted[0],Yvibweighted[1],aY, PasoTiempo);
jjmedina	0:a5367bd4e404	1891
jjmedina	0:a5367bd4e404	1892	if(i2 >= 0)
jjmedina	0:a5367bd4e404	1893	{
jjmedina	0:a5367bd4e404	1894	fprintf(fp3,"%f %f %f %f \n", (t), Xvibweighted[2], Yvibweighted[2], Zvibweighted[2]);
jjmedina	0:a5367bd4e404	1895	}
jjmedina	0:a5367bd4e404	1896
jjmedina	0:a5367bd4e404	1897	//updates
jjmedina	0:a5367bd4e404	1898	aZ[0] = aZ[1];
jjmedina	0:a5367bd4e404	1899	aZ[1] = aZ[2];
jjmedina	0:a5367bd4e404	1900	aX[0] = aX[1];
jjmedina	0:a5367bd4e404	1901	aX[1] = aX[2];
jjmedina	0:a5367bd4e404	1902	aY[0] = aY[1];
jjmedina	0:a5367bd4e404	1903	aY[1] = aY[2];
jjmedina	0:a5367bd4e404	1904	Zvibweighted[0] = Zvibweighted[1];
jjmedina	0:a5367bd4e404	1905	Zvibweighted[1] = Zvibweighted[2];
jjmedina	0:a5367bd4e404	1906	Xvibweighted[0] = Xvibweighted[1];
jjmedina	0:a5367bd4e404	1907	Xvibweighted[1] = Xvibweighted[2];
jjmedina	0:a5367bd4e404	1908	Yvibweighted[0] = Yvibweighted[1];
jjmedina	0:a5367bd4e404	1909	Yvibweighted[1] = Yvibweighted[2];
jjmedina	0:a5367bd4e404	1910	//azfilter1Hz
jjmedina	0:a5367bd4e404	1911
jjmedina	0:a5367bd4e404	1912	}
jjmedina	0:a5367bd4e404	1913
jjmedina	0:a5367bd4e404	1914	}//Fin Hl
jjmedina	0:a5367bd4e404	1915	fclose(fp);
jjmedina	0:a5367bd4e404	1916	fclose(fp3);
jjmedina	0:a5367bd4e404	1917
jjmedina	0:a5367bd4e404	1918
jjmedina	0:a5367bd4e404	1919
jjmedina	0:a5367bd4e404	1920
jjmedina	0:a5367bd4e404	1921	/////////////////////////////////////////
jjmedina	0:a5367bd4e404	1922	//Segund pasada Hh. (High Pass 0.4Hz, Orden 2)
jjmedina	0:a5367bd4e404	1923	strcpy(resultsFile,"/local/W_IV_");
jjmedina	0:a5367bd4e404	1924	strcpy(resultsFileW,"/local/W2_IV_");
jjmedina	0:a5367bd4e404	1925
jjmedina	0:a5367bd4e404	1926	sprintf (NoViajel, "%d", ContadorEnsayos);
jjmedina	0:a5367bd4e404	1927	strcat(resultsFile,NoViajel);
jjmedina	0:a5367bd4e404	1928	strcat(resultsFileW,NoViajel);
jjmedina	0:a5367bd4e404	1929	strcat(resultsFile,".txt");
jjmedina	0:a5367bd4e404	1930	strcat(resultsFileW,".txt");
jjmedina	0:a5367bd4e404	1931
jjmedina	0:a5367bd4e404	1932	//fichero con todos los datos de la función anterior IV_
jjmedina	0:a5367bd4e404	1933	fp = fopen(resultsFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", resultsFile); }
jjmedina	0:a5367bd4e404	1934	//fichero donde guardaré
jjmedina	0:a5367bd4e404	1935	fp3 = fopen(resultsFileW,"w"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", resultsFileW); }
jjmedina	0:a5367bd4e404	1936
jjmedina	0:a5367bd4e404	1937
jjmedina	0:a5367bd4e404	1938
jjmedina	0:a5367bd4e404	1939	//loop principal. Pasada 2. Hh (High Pass 0.4Hz Orden 2)
jjmedina	0:a5367bd4e404	1940	for(unsigned int i = 0; i<(ContadorDatos-(ndes+nmed)); i++)
jjmedina	0:a5367bd4e404	1941	{
jjmedina	0:a5367bd4e404	1942	fscanf(fp,"%f %f %f %f ", &t, &aXYZ[0], &aXYZ[1], &aXYZ[2]);//lee tiempo y 3 aceleraciones X, Y, Z, pasadas por el primer filtro Hl.
jjmedina	0:a5367bd4e404	1943
jjmedina	0:a5367bd4e404	1944	//isofilter
jjmedina	0:a5367bd4e404	1945
jjmedina	0:a5367bd4e404	1946	if ( i==0 )
jjmedina	0:a5367bd4e404	1947	{
jjmedina	0:a5367bd4e404	1948	Zvibweighted[0] = aXYZ[2];
jjmedina	0:a5367bd4e404	1949	Xvibweighted[0] = aXYZ[0];
jjmedina	0:a5367bd4e404	1950	Yvibweighted[0] = aXYZ[1];
jjmedina	0:a5367bd4e404	1951	aZ[0] =aXYZ[2];
jjmedina	0:a5367bd4e404	1952	aX[0] =aXYZ[0];
jjmedina	0:a5367bd4e404	1953	aY[0] =aXYZ[1];
jjmedina	0:a5367bd4e404	1954	fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[0], Yvibweighted[0], Zvibweighted[0]);
jjmedina	0:a5367bd4e404	1955
jjmedina	0:a5367bd4e404	1956	}else if ( i==1 )
jjmedina	0:a5367bd4e404	1957	{
jjmedina	0:a5367bd4e404	1958	Zvibweighted[1] = aXYZ[2];
jjmedina	0:a5367bd4e404	1959	Xvibweighted[1] = aXYZ[0];
jjmedina	0:a5367bd4e404	1960	Yvibweighted[1] = aXYZ[1];
jjmedina	0:a5367bd4e404	1961	aZ[1] =aXYZ[2];
jjmedina	0:a5367bd4e404	1962	aX[1] =aXYZ[0];
jjmedina	0:a5367bd4e404	1963	aY[1] =aXYZ[1];
jjmedina	0:a5367bd4e404	1964	fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[1], Yvibweighted[1], Zvibweighted[1]);
jjmedina	0:a5367bd4e404	1965
jjmedina	0:a5367bd4e404	1966	}else
jjmedina	0:a5367bd4e404	1967	{
jjmedina	0:a5367bd4e404	1968
jjmedina	0:a5367bd4e404	1969	aZ[2] = aXYZ[2];
jjmedina	0:a5367bd4e404	1970	aX[2] = aXYZ[0];
jjmedina	0:a5367bd4e404	1971	aY[2] = aXYZ[1];
jjmedina	0:a5367bd4e404	1972
jjmedina	0:a5367bd4e404	1973	// Zvibweighted[2] = aXYZ[2];//isoFilter_Wk_z_Hh(Zvibweighted[0],Zvibweighted[1],aZ);
jjmedina	0:a5367bd4e404	1974	// Xvibweighted[2] = aXYZ[0];//isoFilter_Wd_xy_Hh(Xvibweighted[0],Xvibweighted[1],aX); //OJO: el filtro High Pass no funciona bien
jjmedina	0:a5367bd4e404	1975	// Yvibweighted[2] = aXYZ[1];//isoFilter_Wd_xy_Hh(Yvibweighted[0],Yvibweighted[1],aY);
jjmedina	0:a5367bd4e404	1976	Zvibweighted[2] = isoFilter_Wk_z_Hh(Zvibweighted[0],Zvibweighted[1],aZ, PasoTiempo);
jjmedina	0:a5367bd4e404	1977	Xvibweighted[2] = isoFilter_Wd_xy_Hh(Xvibweighted[0],Xvibweighted[1],aX, PasoTiempo);
jjmedina	0:a5367bd4e404	1978	Yvibweighted[2] = isoFilter_Wd_xy_Hh(Yvibweighted[0],Yvibweighted[1],aY, PasoTiempo);
jjmedina	0:a5367bd4e404	1979	fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[2], Yvibweighted[2], Zvibweighted[2]);
jjmedina	0:a5367bd4e404	1980	//updates
jjmedina	0:a5367bd4e404	1981	aZ[0] = aZ[1];
jjmedina	0:a5367bd4e404	1982	aZ[1] = aZ[2];
jjmedina	0:a5367bd4e404	1983	aX[0] = aX[1];
jjmedina	0:a5367bd4e404	1984	aX[1] = aX[2];
jjmedina	0:a5367bd4e404	1985	aY[0] = aY[1];
jjmedina	0:a5367bd4e404	1986	aY[1] = aY[2];
jjmedina	0:a5367bd4e404	1987	Zvibweighted[0] = Zvibweighted[1];
jjmedina	0:a5367bd4e404	1988	Zvibweighted[1] = Zvibweighted[2];
jjmedina	0:a5367bd4e404	1989	Xvibweighted[0] = Xvibweighted[1];
jjmedina	0:a5367bd4e404	1990	Xvibweighted[1] = Xvibweighted[2];
jjmedina	0:a5367bd4e404	1991	Yvibweighted[0] = Yvibweighted[1];
jjmedina	0:a5367bd4e404	1992	Yvibweighted[1] = Yvibweighted[2];
jjmedina	0:a5367bd4e404	1993	}
jjmedina	0:a5367bd4e404	1994
jjmedina	0:a5367bd4e404	1995	}//Fin Hh1
jjmedina	0:a5367bd4e404	1996	fclose(fp);
jjmedina	0:a5367bd4e404	1997	fclose(fp3);
jjmedina	0:a5367bd4e404	1998	int rmDVA0 = remove(resultsFile);
jjmedina	0:a5367bd4e404	1999
jjmedina	0:a5367bd4e404	2000
jjmedina	0:a5367bd4e404	2001	/////////////////////////////////////////
jjmedina	0:a5367bd4e404	2002	//Tercera pasada Ht. (Ht filtro transicion)
jjmedina	0:a5367bd4e404	2003	strcpy(resultsFile,"/local/W2_IV_");
jjmedina	0:a5367bd4e404	2004	strcpy(resultsFileW,"/local/W4_IV_");
jjmedina	0:a5367bd4e404	2005
jjmedina	0:a5367bd4e404	2006	sprintf (NoViajel, "%d", ContadorEnsayos);
jjmedina	0:a5367bd4e404	2007	strcat(resultsFile,NoViajel);
jjmedina	0:a5367bd4e404	2008	strcat(resultsFileW,NoViajel);
jjmedina	0:a5367bd4e404	2009	strcat(resultsFile,".txt");
jjmedina	0:a5367bd4e404	2010	strcat(resultsFileW,".txt");
jjmedina	0:a5367bd4e404	2011
jjmedina	0:a5367bd4e404	2012	//fichero con todos los datos de la función anterior IV_
jjmedina	0:a5367bd4e404	2013	fp = fopen(resultsFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", resultsFile); }
jjmedina	0:a5367bd4e404	2014	//fichero donde guardaré
jjmedina	0:a5367bd4e404	2015	fp3 = fopen(resultsFileW,"w"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", resultsFileW); }
jjmedina	0:a5367bd4e404	2016
jjmedina	0:a5367bd4e404	2017
jjmedina	0:a5367bd4e404	2018
jjmedina	0:a5367bd4e404	2019
jjmedina	0:a5367bd4e404	2020	//loop principal. Pasada 3. Ht (Transicion)
jjmedina	0:a5367bd4e404	2021	for(unsigned int i = 0; i<(ContadorDatos-(ndes+nmed)); i++)
jjmedina	0:a5367bd4e404	2022	{
jjmedina	0:a5367bd4e404	2023	fscanf(fp,"%f %f %f %f", &t, &aXYZ[0], &aXYZ[1], &aXYZ[2]);//lee tiempo y 3 aceleraciones X, Y, Z, pasadas por el primer filtro Hl y segundo Hh.
jjmedina	0:a5367bd4e404	2024
jjmedina	0:a5367bd4e404	2025	//isofilter
jjmedina	0:a5367bd4e404	2026
jjmedina	0:a5367bd4e404	2027	if ( i==0 )
jjmedina	0:a5367bd4e404	2028	{
jjmedina	0:a5367bd4e404	2029	Zvibweighted[0] = aXYZ[2];
jjmedina	0:a5367bd4e404	2030	Xvibweighted[0] = aXYZ[0];
jjmedina	0:a5367bd4e404	2031	Yvibweighted[0] = aXYZ[1];
jjmedina	0:a5367bd4e404	2032	aZ[0] =aXYZ[2];
jjmedina	0:a5367bd4e404	2033	aX[0] =aXYZ[0];
jjmedina	0:a5367bd4e404	2034	aY[0] =aXYZ[1];
jjmedina	0:a5367bd4e404	2035	fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[0], Yvibweighted[0], Zvibweighted[0]);
jjmedina	0:a5367bd4e404	2036
jjmedina	0:a5367bd4e404	2037	}else if ( i==1 )
jjmedina	0:a5367bd4e404	2038	{
jjmedina	0:a5367bd4e404	2039	Zvibweighted[1] = aXYZ[2];
jjmedina	0:a5367bd4e404	2040	Xvibweighted[1] = aXYZ[0];
jjmedina	0:a5367bd4e404	2041	Yvibweighted[1] = aXYZ[1];
jjmedina	0:a5367bd4e404	2042	aZ[1] =aXYZ[2];
jjmedina	0:a5367bd4e404	2043	aX[1] =aXYZ[0];
jjmedina	0:a5367bd4e404	2044	aY[1] =aXYZ[1];
jjmedina	0:a5367bd4e404	2045	fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[1], Yvibweighted[1], Zvibweighted[1]);
jjmedina	0:a5367bd4e404	2046
jjmedina	0:a5367bd4e404	2047	}else
jjmedina	0:a5367bd4e404	2048	{
jjmedina	0:a5367bd4e404	2049
jjmedina	0:a5367bd4e404	2050	aZ[2] = aXYZ[2];
jjmedina	0:a5367bd4e404	2051	aX[2] = aXYZ[0];
jjmedina	0:a5367bd4e404	2052	aY[2] = aXYZ[1];
jjmedina	0:a5367bd4e404	2053
jjmedina	0:a5367bd4e404	2054	Zvibweighted[2] = isoFilter_Wk_z_Ht(Zvibweighted[0],Zvibweighted[1],aZ, PasoTiempo);
jjmedina	0:a5367bd4e404	2055	Xvibweighted[2] = isoFilter_Wd_xy_Ht(Xvibweighted[0],Xvibweighted[1],aX, PasoTiempo);
jjmedina	0:a5367bd4e404	2056	Yvibweighted[2] = isoFilter_Wd_xy_Ht(Yvibweighted[0],Yvibweighted[1],aY, PasoTiempo);
jjmedina	0:a5367bd4e404	2057	// Zvibweighted[2] = aXYZ[2];
jjmedina	0:a5367bd4e404	2058	// Xvibweighted[2] = aXYZ[0];
jjmedina	0:a5367bd4e404	2059	// Yvibweighted[2] = aXYZ[1];
jjmedina	0:a5367bd4e404	2060	fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[2], Yvibweighted[2], Zvibweighted[2]);
jjmedina	0:a5367bd4e404	2061	//updates
jjmedina	0:a5367bd4e404	2062	aZ[0] = aZ[1];
jjmedina	0:a5367bd4e404	2063	aZ[1] = aZ[2];
jjmedina	0:a5367bd4e404	2064	aX[0] = aX[1];
jjmedina	0:a5367bd4e404	2065	aX[1] = aX[2];
jjmedina	0:a5367bd4e404	2066	aY[0] = aY[1];
jjmedina	0:a5367bd4e404	2067	aY[1] = aY[2];
jjmedina	0:a5367bd4e404	2068	Zvibweighted[0] = Zvibweighted[1];
jjmedina	0:a5367bd4e404	2069	Zvibweighted[1] = Zvibweighted[2];
jjmedina	0:a5367bd4e404	2070	Xvibweighted[0] = Xvibweighted[1];
jjmedina	0:a5367bd4e404	2071	Xvibweighted[1] = Xvibweighted[2];
jjmedina	0:a5367bd4e404	2072	Yvibweighted[0] = Yvibweighted[1];
jjmedina	0:a5367bd4e404	2073	Yvibweighted[1] = Yvibweighted[2];
jjmedina	0:a5367bd4e404	2074	}
jjmedina	0:a5367bd4e404	2075
jjmedina	0:a5367bd4e404	2076	}//Fin Ht
jjmedina	0:a5367bd4e404	2077	fclose(fp);
jjmedina	0:a5367bd4e404	2078	fclose(fp3);
jjmedina	0:a5367bd4e404	2079	rmDVA0 = remove(resultsFile);
jjmedina	0:a5367bd4e404	2080
jjmedina	0:a5367bd4e404	2081	/////////////////////////////////////////
jjmedina	0:a5367bd4e404	2082	//Cuarta pasada Hs. (Hs filtro subida de escala)
jjmedina	0:a5367bd4e404	2083	strcpy(resultsFile,"/local/W4_IV_");
jjmedina	0:a5367bd4e404	2084	strcpy(resultsFileW,"/local/W5_IV_");
jjmedina	0:a5367bd4e404	2085
jjmedina	0:a5367bd4e404	2086	sprintf (NoViajel, "%d", ContadorEnsayos);
jjmedina	0:a5367bd4e404	2087	strcat(resultsFile,NoViajel);
jjmedina	0:a5367bd4e404	2088	strcat(resultsFileW,NoViajel);
jjmedina	0:a5367bd4e404	2089	strcat(resultsFile,".txt");
jjmedina	0:a5367bd4e404	2090	strcat(resultsFileW,".txt");
jjmedina	0:a5367bd4e404	2091
jjmedina	0:a5367bd4e404	2092	//fichero con todos los datos de la función anterior IV_
jjmedina	0:a5367bd4e404	2093	fp = fopen(resultsFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", resultsFile); }
jjmedina	0:a5367bd4e404	2094	//fichero donde guardaré
jjmedina	0:a5367bd4e404	2095	fp3 = fopen(resultsFileW,"w"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", resultsFileW); }
jjmedina	0:a5367bd4e404	2096
jjmedina	0:a5367bd4e404	2097	//loop principal. Pasada 4. Hs (subida escala)
jjmedina	0:a5367bd4e404	2098
jjmedina	0:a5367bd4e404	2099
jjmedina	0:a5367bd4e404	2100
jjmedina	0:a5367bd4e404	2101
jjmedina	0:a5367bd4e404	2102	for(unsigned int i = 0; i<(ContadorDatos-(ndes+nmed)); i++)
jjmedina	0:a5367bd4e404	2103	{
jjmedina	0:a5367bd4e404	2104	fscanf(fp,"%f %f %f %f", &t, &aXYZ[0], &aXYZ[1], &aXYZ[2]);//lee tiempo y 3 aceleraciones X, Y, Z, pasadas por el primer filtro Hl y segundo Hh y Ht.
jjmedina	0:a5367bd4e404	2105
jjmedina	0:a5367bd4e404	2106	//isofilter
jjmedina	0:a5367bd4e404	2107
jjmedina	0:a5367bd4e404	2108	if ( i==0 )
jjmedina	0:a5367bd4e404	2109	{
jjmedina	0:a5367bd4e404	2110	Zvibweighted[0] = aXYZ[2];
jjmedina	0:a5367bd4e404	2111	Xvibweighted[0] = aXYZ[0];
jjmedina	0:a5367bd4e404	2112	Yvibweighted[0] = aXYZ[1];
jjmedina	0:a5367bd4e404	2113	aZ[0] =aXYZ[2];
jjmedina	0:a5367bd4e404	2114	aX[0] =aXYZ[0];
jjmedina	0:a5367bd4e404	2115	aY[0] =aXYZ[1];
jjmedina	0:a5367bd4e404	2116
jjmedina	0:a5367bd4e404	2117	fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[0], Yvibweighted[0], Zvibweighted[0]);
jjmedina	0:a5367bd4e404	2118
jjmedina	0:a5367bd4e404	2119	}else if ( i==1 )
jjmedina	0:a5367bd4e404	2120	{
jjmedina	0:a5367bd4e404	2121	Zvibweighted[1] = aXYZ[2];
jjmedina	0:a5367bd4e404	2122	Xvibweighted[1] = aXYZ[0];
jjmedina	0:a5367bd4e404	2123	Yvibweighted[1] = aXYZ[1];
jjmedina	0:a5367bd4e404	2124	aZ[1] =aXYZ[2];
jjmedina	0:a5367bd4e404	2125	aX[1] =aXYZ[0];
jjmedina	0:a5367bd4e404	2126	aY[1] =aXYZ[1];
jjmedina	0:a5367bd4e404	2127
jjmedina	0:a5367bd4e404	2128	fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[1], Yvibweighted[1], Zvibweighted[1]);
jjmedina	0:a5367bd4e404	2129
jjmedina	0:a5367bd4e404	2130	}else
jjmedina	0:a5367bd4e404	2131	{
jjmedina	0:a5367bd4e404	2132
jjmedina	0:a5367bd4e404	2133	aZ[2] = aXYZ[2];
jjmedina	0:a5367bd4e404	2134	aX[2] = aXYZ[0];
jjmedina	0:a5367bd4e404	2135	aY[2] = aXYZ[1];
jjmedina	0:a5367bd4e404	2136
jjmedina	0:a5367bd4e404	2137	Zvibweighted[2] = isoFilter_Wk_z_Hs(Zvibweighted[0],Zvibweighted[1],aZ, PasoTiempo);
jjmedina	0:a5367bd4e404	2138	Xvibweighted[2] = aX[2];
jjmedina	0:a5367bd4e404	2139	Yvibweighted[2] = aY[2];
jjmedina	0:a5367bd4e404	2140	// Zvibweighted[2] = aXYZ[2];
jjmedina	0:a5367bd4e404	2141	// Xvibweighted[2] = aX[2];
jjmedina	0:a5367bd4e404	2142	// Yvibweighted[2] = aY[2];
jjmedina	0:a5367bd4e404	2143
jjmedina	0:a5367bd4e404	2144	fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[2], Yvibweighted[2], Zvibweighted[2]);
jjmedina	0:a5367bd4e404	2145	//updates
jjmedina	0:a5367bd4e404	2146	aZ[0] = aZ[1];
jjmedina	0:a5367bd4e404	2147	aZ[1] = aZ[2];
jjmedina	0:a5367bd4e404	2148	aX[0] = aX[1];
jjmedina	0:a5367bd4e404	2149	aX[1] = aX[2];
jjmedina	0:a5367bd4e404	2150	aY[0] = aY[1];
jjmedina	0:a5367bd4e404	2151	aY[1] = aY[2];
jjmedina	0:a5367bd4e404	2152	Zvibweighted[0] = Zvibweighted[1];
jjmedina	0:a5367bd4e404	2153	Zvibweighted[1] = Zvibweighted[2];
jjmedina	0:a5367bd4e404	2154	Xvibweighted[0] = Xvibweighted[1];
jjmedina	0:a5367bd4e404	2155	Xvibweighted[1] = Xvibweighted[2];
jjmedina	0:a5367bd4e404	2156	Yvibweighted[0] = Yvibweighted[1];
jjmedina	0:a5367bd4e404	2157	Yvibweighted[1] = Yvibweighted[2];
jjmedina	0:a5367bd4e404	2158
jjmedina	0:a5367bd4e404	2159	}
jjmedina	0:a5367bd4e404	2160
jjmedina	0:a5367bd4e404	2161	}//Fin Ht
jjmedina	0:a5367bd4e404	2162	fclose(fp);
jjmedina	0:a5367bd4e404	2163	fclose(fp3);
jjmedina	0:a5367bd4e404	2164	rmDVA0 = remove(resultsFile);
jjmedina	0:a5367bd4e404	2165	}
jjmedina	0:a5367bd4e404	2166
jjmedina	0:a5367bd4e404	2167
jjmedina	0:a5367bd4e404	2168	void calculaStep2(unsigned int ContadorEnsayos, unsigned int ContadorDatos)
jjmedina	0:a5367bd4e404	2169	{
jjmedina	0:a5367bd4e404	2170	//calcula step2 previo filtadro deaZ ya filtrada a 10Hz, a 1Hz esta vez.
jjmedina	0:a5367bd4e404	2171	float t;
jjmedina	0:a5367bd4e404	2172	float dZ,vZ,aZfilter10Hz,jerk,aZ[3];
jjmedina	0:a5367bd4e404	2173	float azfilter1Hz[3];
jjmedina	0:a5367bd4e404	2174	//Calculo de step2
jjmedina	0:a5367bd4e404	2175	const float running_time = 1.0;
jjmedina	0:a5367bd4e404	2176
jjmedina	0:a5367bd4e404	2177	int Ai = (int)((0.5running_time1.0e6)/PasoTiempo); //OJO, ESTA VARIABLE VALE 500 PARA F.ADQ = 1000 hZ Y NOS QUEDAMOS SIN RAM
jjmedina	0:a5367bd4e404	2178
jjmedina	0:a5367bd4e404	2179	float azfilter1Hz_1s[2*Ai+1]; //Aceleraciones dentro del periodo de 0.5 segundos para hacer regresion lineal
jjmedina	0:a5367bd4e404	2180	float step2_05s[Ai+1]; //Jerks dentro del periodo de 0.25
jjmedina	0:a5367bd4e404	2181	for (unsigned int i = 0; i<(2*Ai+1); i++) { azfilter1Hz_1s[i] = 0.0; }
jjmedina	0:a5367bd4e404	2182
jjmedina	0:a5367bd4e404	2183	for (unsigned int i = 0; i<(Ai+1); i++) { step2_05s[i] = 0.0; }
jjmedina	0:a5367bd4e404	2184
jjmedina	0:a5367bd4e404	2185	//Archivos de lectura y escritura
jjmedina	0:a5367bd4e404	2186
jjmedina	0:a5367bd4e404	2187	FILE * fp;
jjmedina	0:a5367bd4e404	2188	FILE * fp2;
jjmedina	0:a5367bd4e404	2189
jjmedina	0:a5367bd4e404	2190	char NoViajel[2];
jjmedina	0:a5367bd4e404	2191	char dispVelFile[50]; char velMod[50];
jjmedina	0:a5367bd4e404	2192
jjmedina	0:a5367bd4e404	2193	strcpy(dispVelFile,"/local/dvaj_");
jjmedina	0:a5367bd4e404	2194	strcpy(velMod,"/local/dvaja_");
jjmedina	0:a5367bd4e404	2195
jjmedina	0:a5367bd4e404	2196	sprintf (NoViajel, "%d", ContadorEnsayos);
jjmedina	0:a5367bd4e404	2197	strcat(dispVelFile,NoViajel); strcat(velMod,NoViajel);
jjmedina	0:a5367bd4e404	2198	strcat(dispVelFile,".txt"); strcat(velMod,".txt");
jjmedina	0:a5367bd4e404	2199
jjmedina	0:a5367bd4e404	2200	fp = fopen(dispVelFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", dispVelFile); }
jjmedina	0:a5367bd4e404	2201	fp2 = fopen(velMod,"w"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", velMod); }
jjmedina	0:a5367bd4e404	2202
jjmedina	0:a5367bd4e404	2203	//const float fs = (1000000.0/PasoTiempo);//frecuencia de muestreo debe valer 320
jjmedina	0:a5367bd4e404	2204
jjmedina	0:a5367bd4e404	2205	//Primera pasada. calcula azfilter1Hz
jjmedina	0:a5367bd4e404	2206
jjmedina	0:a5367bd4e404	2207	int nDatos = (ContadorDatos-(ndes+nmed));
jjmedina	0:a5367bd4e404	2208	if (nDatos < 0) {nDatos = 0;}
jjmedina	0:a5367bd4e404	2209
jjmedina	0:a5367bd4e404	2210	for(unsigned int i = 0; i<nDatos; i++)
jjmedina	0:a5367bd4e404	2211	{
jjmedina	0:a5367bd4e404	2212	//fscanf(fp,"%f %f %f %f %f %f", &t, &dZ, &vZ, &aZ, &aX, &aY);//aZ filtered, aX y aY raw.
jjmedina	0:a5367bd4e404	2213	fscanf(fp,"%f %f %f %f %f", &t, &dZ, &vZ, &aZfilter10Hz, &jerk);
jjmedina	0:a5367bd4e404	2214	//
jjmedina	0:a5367bd4e404	2215	//isofilter
jjmedina	0:a5367bd4e404	2216	if ( i==0 )
jjmedina	0:a5367bd4e404	2217	{
jjmedina	0:a5367bd4e404	2218	//azfilter1Hz
jjmedina	0:a5367bd4e404	2219	aZ[0] = aZfilter10Hz;
jjmedina	0:a5367bd4e404	2220	azfilter1Hz[0] = aZfilter10Hz;
jjmedina	0:a5367bd4e404	2221	fprintf(fp2,"%f %f %f %f %f %f \n", t, dZ, vZ, aZfilter10Hz, jerk, azfilter1Hz[0]);
jjmedina	0:a5367bd4e404	2222
jjmedina	0:a5367bd4e404	2223	}else if ( i==1 )
jjmedina	0:a5367bd4e404	2224	{
jjmedina	0:a5367bd4e404	2225	//azfilter1Hz
jjmedina	0:a5367bd4e404	2226	aZ[1] = aZfilter10Hz;
jjmedina	0:a5367bd4e404	2227	azfilter1Hz[1] = aZfilter10Hz;
jjmedina	0:a5367bd4e404	2228
jjmedina	0:a5367bd4e404	2229	fprintf(fp2,"%f %f %f %f %f %f \n", t, dZ, vZ, aZfilter10Hz, jerk, azfilter1Hz[1]);
jjmedina	0:a5367bd4e404	2230
jjmedina	0:a5367bd4e404	2231	}else
jjmedina	0:a5367bd4e404	2232	{
jjmedina	0:a5367bd4e404	2233	//azfilter1Hz
jjmedina	0:a5367bd4e404	2234	aZ[2] = aZfilter10Hz;
jjmedina	0:a5367bd4e404	2235	if ( FrecAdq == 320 )
jjmedina	0:a5367bd4e404	2236	{
jjmedina	0:a5367bd4e404	2237	azfilter1Hz[2] = toLowPass1HzO2_320(azfilter1Hz[0],azfilter1Hz[1],aZ);
jjmedina	0:a5367bd4e404	2238
jjmedina	0:a5367bd4e404	2239	}else if ( FrecAdq == 1000 )
jjmedina	0:a5367bd4e404	2240	{
jjmedina	0:a5367bd4e404	2241	azfilter1Hz[2] = toLowPass1HzO2_1000(azfilter1Hz[0],azfilter1Hz[1],aZ);
jjmedina	0:a5367bd4e404	2242	}else//seguridad
jjmedina	0:a5367bd4e404	2243	{
jjmedina	0:a5367bd4e404	2244	azfilter1Hz[2] = toLowPass1HzO2_320(azfilter1Hz[0],azfilter1Hz[1],aZ);
jjmedina	0:a5367bd4e404	2245	}
jjmedina	0:a5367bd4e404	2246
jjmedina	0:a5367bd4e404	2247	fprintf(fp2,"%f %f %f %f %f %f \n", t, dZ, vZ, aZfilter10Hz, jerk, azfilter1Hz[2]);
jjmedina	0:a5367bd4e404	2248	//updates
jjmedina	0:a5367bd4e404	2249	aZ[0] = aZ[1];
jjmedina	0:a5367bd4e404	2250	aZ[1] = aZ[2];
jjmedina	0:a5367bd4e404	2251	//azfilter1Hz
jjmedina	0:a5367bd4e404	2252	azfilter1Hz[0] = azfilter1Hz[1];
jjmedina	0:a5367bd4e404	2253	azfilter1Hz[1] = azfilter1Hz[2];
jjmedina	0:a5367bd4e404	2254	}
jjmedina	0:a5367bd4e404	2255
jjmedina	0:a5367bd4e404	2256	}//fin pasada
jjmedina	0:a5367bd4e404	2257	fclose(fp);
jjmedina	0:a5367bd4e404	2258	fclose(fp2);
jjmedina	0:a5367bd4e404	2259	//int rmDVA0 = remove(dispVelFile);//elimina dvaj_
jjmedina	0:a5367bd4e404	2260
jjmedina	0:a5367bd4e404	2261	//segunda pasada. Calcula step2
jjmedina	0:a5367bd4e404	2262	strcpy(dispVelFile,"/local/dvaja_");
jjmedina	0:a5367bd4e404	2263	strcpy(velMod,"/local/dvaj2_");
jjmedina	0:a5367bd4e404	2264
jjmedina	0:a5367bd4e404	2265	sprintf (NoViajel, "%d", ContadorEnsayos);
jjmedina	0:a5367bd4e404	2266	strcat(dispVelFile,NoViajel); strcat(velMod,NoViajel);
jjmedina	0:a5367bd4e404	2267	strcat(dispVelFile,".txt"); strcat(velMod,".txt");
jjmedina	0:a5367bd4e404	2268
jjmedina	0:a5367bd4e404	2269	fp = fopen(dispVelFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", dispVelFile); }
jjmedina	0:a5367bd4e404	2270	fp2 = fopen(velMod,"w"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", velMod); }
jjmedina	0:a5367bd4e404	2271
jjmedina	0:a5367bd4e404	2272	if (nDatos < 0) {nDatos = 0;}
jjmedina	0:a5367bd4e404	2273
jjmedina	0:a5367bd4e404	2274	for(unsigned int i = 0; i<nDatos; i++)
jjmedina	0:a5367bd4e404	2275	{
jjmedina	0:a5367bd4e404	2276	//fscanf(fp,"%f %f %f %f %f %f", &t, &dZ, &vZ, &aZ, &aX, &aY);//aZ filtered, aX y aY raw.
jjmedina	0:a5367bd4e404	2277	fscanf(fp,"%f %f %f %f %f %f", &t, &dZ, &vZ, &aZfilter10Hz, &jerk, &azfilter1Hz[2]);
jjmedina	0:a5367bd4e404	2278	//Calculo de step2 por regresion
jjmedina	0:a5367bd4e404	2279
jjmedina	0:a5367bd4e404	2280	for(unsigned int j = 0; j<(2*Ai+1); j++) { azfilter1Hz_1s[j] = azfilter1Hz_1s[j+1]; }
jjmedina	0:a5367bd4e404	2281	azfilter1Hz_1s[2*Ai] = azfilter1Hz[2];
jjmedina	0:a5367bd4e404	2282	for(unsigned int j = 0; j<(Ai+1); j++) { step2_05s[j] = step2_05s[j+1]; }
jjmedina	0:a5367bd4e404	2283	step2_05s[Ai] = RegresionJerk(azfilter1Hz_1s, 2*Ai+1);
jjmedina	0:a5367bd4e404	2284
jjmedina	0:a5367bd4e404	2285	fprintf(fp2,"%f %f %f %f %f %f \n", t, dZ, vZ, aZfilter10Hz, jerk, step2_05s[0]);
jjmedina	0:a5367bd4e404	2286	}
jjmedina	0:a5367bd4e404	2287
jjmedina	0:a5367bd4e404	2288	fclose(fp);
jjmedina	0:a5367bd4e404	2289	fclose(fp2);
jjmedina	0:a5367bd4e404	2290	int rmDVA1 = remove(dispVelFile);
jjmedina	0:a5367bd4e404	2291
jjmedina	0:a5367bd4e404	2292	}
jjmedina	0:a5367bd4e404	2293
jjmedina	0:a5367bd4e404	2294	void calculaCarNCar(unsigned int ContadorEnsayos, unsigned int ContadorDatos, int PasoTiempo)
jjmedina	0:a5367bd4e404	2295	{
jjmedina	0:a5367bd4e404	2296	float t;
jjmedina	0:a5367bd4e404	2297	float dZ,vZ,aZfilter10Hz,jerk,step2;
jjmedina	0:a5367bd4e404	2298
jjmedina	0:a5367bd4e404	2299	const float timeStep = (float)(PasoTiempo/1000000.0);
jjmedina	0:a5367bd4e404	2300	const float Atadq = timeStep;
jjmedina	0:a5367bd4e404	2301
jjmedina	0:a5367bd4e404	2302	//Archivos de lectura y escritura
jjmedina	0:a5367bd4e404	2303	FILE * fp;
jjmedina	0:a5367bd4e404	2304
jjmedina	0:a5367bd4e404	2305	char NoViajel[2];
jjmedina	0:a5367bd4e404	2306	char dispVelFile[50];
jjmedina	0:a5367bd4e404	2307
jjmedina	0:a5367bd4e404	2308	strcpy(dispVelFile,"/local/dvaj2_");
jjmedina	0:a5367bd4e404	2309	sprintf (NoViajel, "%d", ContadorEnsayos);
jjmedina	0:a5367bd4e404	2310	strcat(dispVelFile,NoViajel);
jjmedina	0:a5367bd4e404	2311	strcat(dispVelFile,".txt");
jjmedina	0:a5367bd4e404	2312
jjmedina	0:a5367bd4e404	2313	fp = fopen(dispVelFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", dispVelFile); }
jjmedina	0:a5367bd4e404	2314
jjmedina	0:a5367bd4e404	2315	int control(0);
jjmedina	0:a5367bd4e404	2316	vector <int> regiones;
jjmedina	0:a5367bd4e404	2317
jjmedina	0:a5367bd4e404	2318	int nDatos = (ContadorDatos-(ndes+nmed));
jjmedina	0:a5367bd4e404	2319	if (nDatos < 0) {nDatos = 0;}
jjmedina	0:a5367bd4e404	2320
jjmedina	0:a5367bd4e404	2321	for(int i = 0; i<nDatos; i++)
jjmedina	0:a5367bd4e404	2322	{
jjmedina	0:a5367bd4e404	2323
jjmedina	0:a5367bd4e404	2324	fscanf(fp,"%f %f %f %f %f %f \n", &t, &dZ, &vZ, &aZfilter10Hz, &jerk, &step2);//aZ filtered, aX y aY raw.
jjmedina	0:a5367bd4e404	2325
jjmedina	0:a5367bd4e404	2326	//Step 3: Identifica slopes con valor mayor que 0.3 m/s^3
jjmedina	0:a5367bd4e404	2327	if ( (i >= Bo0) && (i <= Bo3) )
jjmedina	0:a5367bd4e404	2328	{
jjmedina	0:a5367bd4e404	2329	if ((fabs(step2)>=0.3)&&(control==0))
jjmedina	0:a5367bd4e404	2330	{
jjmedina	0:a5367bd4e404	2331
jjmedina	0:a5367bd4e404	2332	regiones.push_back(i-((int)(0.5/timeStep)));//correccion indice por retraso step2 en 0.5 segundos OJO
jjmedina	0:a5367bd4e404	2333	// regiones.push_back(i);//correccion indice por retraso step2 en 0.5 segundos OJO
jjmedina	0:a5367bd4e404	2334	control=1;
jjmedina	0:a5367bd4e404	2335	}
jjmedina	0:a5367bd4e404	2336	else if ((fabs(step2)<=0.3)&&(control==1))
jjmedina	0:a5367bd4e404	2337	{
jjmedina	0:a5367bd4e404	2338	control=0;
jjmedina	0:a5367bd4e404	2339	regiones.push_back(i-1-((int)(0.5/timeStep)));//correccion indice por retraso step2 en 0.5 segundos
jjmedina	0:a5367bd4e404	2340	// regiones.push_back(i-1);//correccion indice por retraso step2 en 0.5 segundos
jjmedina	0:a5367bd4e404	2341	}
jjmedina	0:a5367bd4e404	2342	}
jjmedina	0:a5367bd4e404	2343	}//fin loop AQUI---abajo, cambiar time.at(regiones.at(2*i)) por tiempo sabiendo el paso etc.
jjmedina	0:a5367bd4e404	2344
jjmedina	0:a5367bd4e404	2345	//Step 4: Add 0.5 s . Hecho en el bucle de abajo. al primero le resto 0.5 y
jjmedina	0:a5367bd4e404	2346	// al segundo se lo sumo. Para los indices como conozco Atadq, sumo y resto
jjmedina	0:a5367bd4e404	2347	// 0.5/Atadq
jjmedina	0:a5367bd4e404	2348	Nregiones = (int)(regiones.size()/2.0);
jjmedina	0:a5367bd4e404	2349	//region [t1 t2 index(t1) index(t1)]
jjmedina	0:a5367bd4e404	2350
jjmedina	0:a5367bd4e404	2351	if(Nregiones > 0)
jjmedina	0:a5367bd4e404	2352	{
jjmedina	0:a5367bd4e404	2353	region = new float* [Nregiones];
jjmedina	0:a5367bd4e404	2354	for(unsigned int i = 0; i < Nregiones; i++)
jjmedina	0:a5367bd4e404	2355	region[i] = new float[4];
jjmedina	0:a5367bd4e404	2356
jjmedina	0:a5367bd4e404	2357
jjmedina	0:a5367bd4e404	2358	for (unsigned int i = 0; i < Nregiones; i++)
jjmedina	0:a5367bd4e404	2359	{
jjmedina	0:a5367bd4e404	2360	float factorT(0.5);
jjmedina	0:a5367bd4e404	2361	// while ( ((timeStepregiones.at(2i)-factorT)<0) \|\| ((timeStepregiones.at(2i +1)+factorT)>time.at(time.size()-1)) )
jjmedina	0:a5367bd4e404	2362	while ( ((timeStepregiones.at(2i)-factorT)<0) \|\| ((timeStepregiones.at(2i +1)+factorT)>timeStep*Bo3) )
jjmedina	0:a5367bd4e404	2363	{
jjmedina	0:a5367bd4e404	2364	factorT = factorT - 0.1;
jjmedina	0:a5367bd4e404	2365	}
jjmedina	0:a5367bd4e404	2366	region[i][0] = timeStepregiones.at(2i)-factorT;
jjmedina	0:a5367bd4e404	2367	region[i][2] = regiones.at(2*i)-factorT/Atadq;
jjmedina	0:a5367bd4e404	2368
jjmedina	0:a5367bd4e404	2369	region[i][1] = timeStepregiones.at(2i +1)+factorT;
jjmedina	0:a5367bd4e404	2370	region[i][3] = regiones.at(2*i + 1)+factorT/Atadq;
jjmedina	0:a5367bd4e404	2371	}
jjmedina	0:a5367bd4e404	2372	// Step 5: cada fila de region (=Nregiones) es una region de non-constant
jjmedina	0:a5367bd4e404	2373	// acceleration
jjmedina	0:a5367bd4e404	2374	// Step 6: define regiones de constant acceleration (car)
jjmedina	0:a5367bd4e404	2375
jjmedina	0:a5367bd4e404	2376	int cont = 0;
jjmedina	0:a5367bd4e404	2377
jjmedina	0:a5367bd4e404	2378	for (unsigned int i=0; i < (Nregiones-1); i++)
jjmedina	0:a5367bd4e404	2379	{
jjmedina	0:a5367bd4e404	2380	if ( region[i+1][2] > region[i][3] )
jjmedina	0:a5367bd4e404	2381	{
jjmedina	0:a5367bd4e404	2382
jjmedina	0:a5367bd4e404	2383	regionconst[cont][0]=region[i][1];
jjmedina	0:a5367bd4e404	2384	regionconst[cont][1]=region[i+1][0];
jjmedina	0:a5367bd4e404	2385	regionconst[cont][2]=region[i][3];
jjmedina	0:a5367bd4e404	2386	regionconst[cont][3]=region[i+1][2];
jjmedina	0:a5367bd4e404	2387	cont++;
jjmedina	0:a5367bd4e404	2388	}
jjmedina	0:a5367bd4e404	2389	}
jjmedina	0:a5367bd4e404	2390	Nregionesconst = cont;
jjmedina	0:a5367bd4e404	2391	}
jjmedina	0:a5367bd4e404	2392	else
jjmedina	0:a5367bd4e404	2393	{
jjmedina	0:a5367bd4e404	2394	Nregionesconst = 0;
jjmedina	0:a5367bd4e404	2395	}
jjmedina	0:a5367bd4e404	2396
jjmedina	0:a5367bd4e404	2397	fclose(fp);
jjmedina	0:a5367bd4e404	2398	int rmDVA0 = remove(dispVelFile);
jjmedina	0:a5367bd4e404	2399
jjmedina	0:a5367bd4e404	2400	}
jjmedina	0:a5367bd4e404	2401
jjmedina	0:a5367bd4e404	2402	void calculaZXYvibweighted( unsigned int ContadorEnsayos, unsigned int ContadorDatos)
jjmedina	0:a5367bd4e404	2403	{
jjmedina	0:a5367bd4e404	2404	//aux data
jjmedina	0:a5367bd4e404	2405	float Zvibweighted_max(0.0);
jjmedina	0:a5367bd4e404	2406	float Zvibweighted_min(0.0);
jjmedina	0:a5367bd4e404	2407	float Zvibweighted_max_ncar(0.0);
jjmedina	0:a5367bd4e404	2408	float Zvibweighted_min_ncar(0.0);
jjmedina	0:a5367bd4e404	2409	float Xvibweighted_max(0.0);
jjmedina	0:a5367bd4e404	2410	float Xvibweighted_min(0.0);
jjmedina	0:a5367bd4e404	2411	float Yvibweighted_max(0.0);
jjmedina	0:a5367bd4e404	2412	float Yvibweighted_min(0.0);
jjmedina	0:a5367bd4e404	2413	int signoCompara;
jjmedina	0:a5367bd4e404	2414	int signoCompara_ncar;
jjmedina	0:a5367bd4e404	2415	int signoCompara_X;
jjmedina	0:a5367bd4e404	2416	int signoCompara_Y;
jjmedina	0:a5367bd4e404	2417	int signoControl;
jjmedina	0:a5367bd4e404	2418	int signoControl_X;
jjmedina	0:a5367bd4e404	2419	int signoControl_Y;
jjmedina	0:a5367bd4e404	2420	int contadorCeros(0);
jjmedina	0:a5367bd4e404	2421	int contadorCeros_ncar(0);
jjmedina	0:a5367bd4e404	2422	int contadorCeros_X(0);
jjmedina	0:a5367bd4e404	2423	int contadorCeros_Y(0);
jjmedina	0:a5367bd4e404	2424
jjmedina	0:a5367bd4e404	2425
jjmedina	0:a5367bd4e404	2426	float difZ(0.0);
jjmedina	0:a5367bd4e404	2427	float difX(0.0);
jjmedina	0:a5367bd4e404	2428	float difY(0.0);
jjmedina	0:a5367bd4e404	2429	int contador_peaktopeak_car(0);
jjmedina	0:a5367bd4e404	2430	int contador_peaktopeak_X(0);
jjmedina	0:a5367bd4e404	2431	int contador_peaktopeak_Y(0);
jjmedina	0:a5367bd4e404	2432
jjmedina	0:a5367bd4e404	2433	int marca(-1);//-1 = no
jjmedina	0:a5367bd4e404	2434
jjmedina	0:a5367bd4e404	2435	//read data from file
jjmedina	0:a5367bd4e404	2436	float Xvibweighted;
jjmedina	0:a5367bd4e404	2437	float Yvibweighted;
jjmedina	0:a5367bd4e404	2438	float Zvibweighted;
jjmedina	0:a5367bd4e404	2439	float t;
jjmedina	0:a5367bd4e404	2440
jjmedina	0:a5367bd4e404	2441
jjmedina	0:a5367bd4e404	2442	//Archivos de lectura y escritura
jjmedina	0:a5367bd4e404	2443	FILE * fp;
jjmedina	0:a5367bd4e404	2444	FILE * fp2;
jjmedina	0:a5367bd4e404	2445	FILE * fp3;
jjmedina	0:a5367bd4e404	2446	FILE * fp4;
jjmedina	0:a5367bd4e404	2447
jjmedina	0:a5367bd4e404	2448	char NoViajel[2];
jjmedina	0:a5367bd4e404	2449	char dispVelFile[50];
jjmedina	0:a5367bd4e404	2450	char A95file[50];
jjmedina	0:a5367bd4e404	2451	char A95fileX[50];
jjmedina	0:a5367bd4e404	2452	char A95fileY[50];
jjmedina	0:a5367bd4e404	2453
jjmedina	0:a5367bd4e404	2454	strcpy(dispVelFile,"/local/W5_IV_");
jjmedina	0:a5367bd4e404	2455	strcpy(A95file,"/local/A95_IV");
jjmedina	0:a5367bd4e404	2456	strcpy(A95fileX,"/local/AX_IV_");
jjmedina	0:a5367bd4e404	2457	strcpy(A95fileY,"/local/AY_IV_");
jjmedina	0:a5367bd4e404	2458	sprintf (NoViajel, "%d", ContadorEnsayos);
jjmedina	0:a5367bd4e404	2459	strcat(dispVelFile,NoViajel);
jjmedina	0:a5367bd4e404	2460	strcat(A95file,NoViajel);
jjmedina	0:a5367bd4e404	2461	strcat(A95fileX,NoViajel);
jjmedina	0:a5367bd4e404	2462	strcat(A95fileY,NoViajel);
jjmedina	0:a5367bd4e404	2463	strcat(dispVelFile,".txt");
jjmedina	0:a5367bd4e404	2464	strcat(A95file,".txt");
jjmedina	0:a5367bd4e404	2465	strcat(A95fileX,".txt");
jjmedina	0:a5367bd4e404	2466	strcat(A95fileY,".txt");
jjmedina	0:a5367bd4e404	2467
jjmedina	0:a5367bd4e404	2468
jjmedina	0:a5367bd4e404	2469	fp = fopen(dispVelFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", dispVelFile); }
jjmedina	0:a5367bd4e404	2470	fp2 = fopen(A95file,"w"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95file); }
jjmedina	0:a5367bd4e404	2471	fp3 = fopen(A95fileX,"w"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileX); }
jjmedina	0:a5367bd4e404	2472	fp4 = fopen(A95fileY,"w"); if(fp4 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileY); }
jjmedina	0:a5367bd4e404	2473
jjmedina	0:a5367bd4e404	2474	int nDatos = (ContadorDatos-(ndes+nmed));
jjmedina	0:a5367bd4e404	2475	if (nDatos < 0) {nDatos = 0;}
jjmedina	0:a5367bd4e404	2476
jjmedina	0:a5367bd4e404	2477	for(unsigned int i = 0; i<nDatos; i++)//-------------------------------------------------------------MAIN LOOP
jjmedina	0:a5367bd4e404	2478	{
jjmedina	0:a5367bd4e404	2479	fscanf(fp,"%f %f %f %f", &t, &Xvibweighted, &Yvibweighted, &Zvibweighted);//lee tiempo y 3 aceleraciones ponderadas X, Y, Z
jjmedina	0:a5367bd4e404	2480
jjmedina	0:a5367bd4e404	2481	///////////////////////////////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	2482	////////////////Zvib///////////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	2483	///////////////////////////////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	2484	//signo
jjmedina	0:a5367bd4e404	2485	signoControl = (int)(Zvibweighted/fabs(Zvibweighted));//+1 positivo, -1 negativo
jjmedina	0:a5367bd4e404	2486
jjmedina	0:a5367bd4e404	2487	//////////////////////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	2488	//Constant acceleration Region (subindex=car). Maximum peak to peak y A95 peak to peak
jjmedina	0:a5367bd4e404	2489	//comprueba que esta dentro de alguna de las fronteras
jjmedina	0:a5367bd4e404	2490	if ( Nregionesconst == 0 )
jjmedina	0:a5367bd4e404	2491	{
jjmedina	0:a5367bd4e404	2492	max_peaktopeak_car_ = 0.0;
jjmedina	0:a5367bd4e404	2493	A95_peaktopeak_car_ = 0.0;
jjmedina	0:a5367bd4e404	2494	}else
jjmedina	0:a5367bd4e404	2495	{
jjmedina	0:a5367bd4e404	2496	for (int j = 0 ; j < Nregionesconst ; j++)
jjmedina	0:a5367bd4e404	2497	{
jjmedina	0:a5367bd4e404	2498	if ( i == ((int)regionconst[j][2]) )
jjmedina	0:a5367bd4e404	2499	{
jjmedina	0:a5367bd4e404	2500	signoCompara = (int)(Zvibweighted/fabs(Zvibweighted));//+1 positivo, -1 negativo
jjmedina	0:a5367bd4e404	2501	}
jjmedina	0:a5367bd4e404	2502	if ( (((int)regionconst[j][2]) < i) && (i < (int)regionconst[j][3]) )
jjmedina	0:a5367bd4e404	2503	{
jjmedina	0:a5367bd4e404	2504	if ( (signoControl-signoCompara) != 0 )
jjmedina	0:a5367bd4e404	2505	{
jjmedina	0:a5367bd4e404	2506	contadorCeros++;
jjmedina	0:a5367bd4e404	2507	signoCompara = signoControl;
jjmedina	0:a5367bd4e404	2508	}
jjmedina	0:a5367bd4e404	2509	if ( (1 <= contadorCeros) && (contadorCeros < 3) )
jjmedina	0:a5367bd4e404	2510	{
jjmedina	0:a5367bd4e404	2511	if ( Zvibweighted > Zvibweighted_max )
jjmedina	0:a5367bd4e404	2512	{
jjmedina	0:a5367bd4e404	2513	Zvibweighted_max = Zvibweighted;
jjmedina	0:a5367bd4e404	2514	}
jjmedina	0:a5367bd4e404	2515	if ( Zvibweighted < Zvibweighted_min )
jjmedina	0:a5367bd4e404	2516	{
jjmedina	0:a5367bd4e404	2517	Zvibweighted_min = Zvibweighted;
jjmedina	0:a5367bd4e404	2518	}
jjmedina	0:a5367bd4e404	2519	}else if (contadorCeros == 3)
jjmedina	0:a5367bd4e404	2520	{
jjmedina	0:a5367bd4e404	2521	contadorCeros = 0;
jjmedina	0:a5367bd4e404	2522	difZ = (Zvibweighted_max - Zvibweighted_min);
jjmedina	0:a5367bd4e404	2523	if ( difZ > max_peaktopeak_car_ ){max_peaktopeak_car_ = difZ;}
jjmedina	0:a5367bd4e404	2524	//almacena para A95
jjmedina	0:a5367bd4e404	2525	fprintf(fp2,"%f \n", difZ);
jjmedina	0:a5367bd4e404	2526
jjmedina	0:a5367bd4e404	2527	contador_peaktopeak_car++;
jjmedina	0:a5367bd4e404	2528	Zvibweighted_max = 0.0;
jjmedina	0:a5367bd4e404	2529	Zvibweighted_min = 0.0;
jjmedina	0:a5367bd4e404	2530	}
jjmedina	0:a5367bd4e404	2531	}
jjmedina	0:a5367bd4e404	2532	}//for Nregionesconst
jjmedina	0:a5367bd4e404	2533	}//else
jjmedina	0:a5367bd4e404	2534
jjmedina	0:a5367bd4e404	2535
jjmedina	0:a5367bd4e404	2536	//////////////////////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	2537	//Non-constant acceleration Region (subindex=ncar). Maximum peak to peak
jjmedina	0:a5367bd4e404	2538	//comprueba que esta dentro de alguna de las fronteras
jjmedina	0:a5367bd4e404	2539	if ( Nregiones == 0 )
jjmedina	0:a5367bd4e404	2540	{
jjmedina	0:a5367bd4e404	2541	max_peaktopeak_ncar_ = 0.0;
jjmedina	0:a5367bd4e404	2542	}else
jjmedina	0:a5367bd4e404	2543	{
jjmedina	0:a5367bd4e404	2544	for (int j = 0 ; j < Nregiones ; j++)
jjmedina	0:a5367bd4e404	2545	{
jjmedina	0:a5367bd4e404	2546	if ( i == ((int)region[j][2]) )
jjmedina	0:a5367bd4e404	2547	{
jjmedina	0:a5367bd4e404	2548	signoCompara_ncar = (int)(Zvibweighted/fabs(Zvibweighted));//+1 positivo, -1 negativo
jjmedina	0:a5367bd4e404	2549	marca = j;
jjmedina	0:a5367bd4e404	2550	}
jjmedina	0:a5367bd4e404	2551	if ( (((int)region[j][2]) < i) && (i < (int)region[j][3]) )
jjmedina	0:a5367bd4e404	2552	{
jjmedina	0:a5367bd4e404	2553	if ( (signoControl-signoCompara_ncar) != 0 )
jjmedina	0:a5367bd4e404	2554	{
jjmedina	0:a5367bd4e404	2555	contadorCeros_ncar++;
jjmedina	0:a5367bd4e404	2556	signoCompara_ncar = signoControl;
jjmedina	0:a5367bd4e404	2557	}
jjmedina	0:a5367bd4e404	2558	if ( (1 <= contadorCeros_ncar) && (contadorCeros_ncar < 3))// && (marca == j) )
jjmedina	0:a5367bd4e404	2559	{
jjmedina	0:a5367bd4e404	2560	if ( (Zvibweighted > Zvibweighted_max_ncar) && (marca == j) )
jjmedina	0:a5367bd4e404	2561	{
jjmedina	0:a5367bd4e404	2562	Zvibweighted_max_ncar = Zvibweighted;
jjmedina	0:a5367bd4e404	2563	}
jjmedina	0:a5367bd4e404	2564	if ( (Zvibweighted < Zvibweighted_min_ncar) && (marca == j) )
jjmedina	0:a5367bd4e404	2565	{
jjmedina	0:a5367bd4e404	2566	Zvibweighted_min_ncar = Zvibweighted;
jjmedina	0:a5367bd4e404	2567	}
jjmedina	0:a5367bd4e404	2568	}else
jjmedina	0:a5367bd4e404	2569	{
jjmedina	0:a5367bd4e404	2570	contadorCeros_ncar = 0; //marca = -1;
jjmedina	0:a5367bd4e404	2571	if ( (Zvibweighted_max_ncar - Zvibweighted_min_ncar) > max_peaktopeak_ncar_ ){max_peaktopeak_ncar_ = (Zvibweighted_max_ncar - Zvibweighted_min_ncar);}
jjmedina	0:a5367bd4e404	2572	Zvibweighted_max_ncar = 0.0;
jjmedina	0:a5367bd4e404	2573	Zvibweighted_min_ncar = 0.0;
jjmedina	0:a5367bd4e404	2574	}
jjmedina	0:a5367bd4e404	2575	}
jjmedina	0:a5367bd4e404	2576	}//for Nregiones
jjmedina	0:a5367bd4e404	2577	}//else
jjmedina	0:a5367bd4e404	2578
jjmedina	0:a5367bd4e404	2579	//////////////////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	2580	////////Xvib/////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	2581	//////////////////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	2582	//signo
jjmedina	0:a5367bd4e404	2583	signoControl_X = (int)(Xvibweighted/fabs(Xvibweighted));//+1 positivo, -1 negativo
jjmedina	0:a5367bd4e404	2584	if ( i == Bo1 )
jjmedina	0:a5367bd4e404	2585	{
jjmedina	0:a5367bd4e404	2586	signoCompara_X = (int)(Xvibweighted/fabs(Xvibweighted));//+1 positivo, -1 negativo
jjmedina	0:a5367bd4e404	2587	}
jjmedina	0:a5367bd4e404	2588	if ( Bo1 == Bo2 )
jjmedina	0:a5367bd4e404	2589	{
jjmedina	0:a5367bd4e404	2590	max_peaktopeak_X_ = 0.0;
jjmedina	0:a5367bd4e404	2591	}else
jjmedina	0:a5367bd4e404	2592	{
jjmedina	0:a5367bd4e404	2593	if (( Bo1 < i) && (i < Bo2 ))
jjmedina	0:a5367bd4e404	2594	{
jjmedina	0:a5367bd4e404	2595	if ( (signoControl_X-signoCompara_X) != 0 )
jjmedina	0:a5367bd4e404	2596	{
jjmedina	0:a5367bd4e404	2597	contadorCeros_X++;
jjmedina	0:a5367bd4e404	2598	signoCompara_X = signoControl_X;
jjmedina	0:a5367bd4e404	2599	}
jjmedina	0:a5367bd4e404	2600	if ( (1 <= contadorCeros_X) && (contadorCeros_X < 3) )
jjmedina	0:a5367bd4e404	2601	{
jjmedina	0:a5367bd4e404	2602	if ( Xvibweighted > Xvibweighted_max )
jjmedina	0:a5367bd4e404	2603	{
jjmedina	0:a5367bd4e404	2604	Xvibweighted_max = Xvibweighted;
jjmedina	0:a5367bd4e404	2605	}
jjmedina	0:a5367bd4e404	2606	if ( Xvibweighted < Xvibweighted_min )
jjmedina	0:a5367bd4e404	2607	{
jjmedina	0:a5367bd4e404	2608	Xvibweighted_min = Xvibweighted;
jjmedina	0:a5367bd4e404	2609	}
jjmedina	0:a5367bd4e404	2610	}else if (contadorCeros_X == 3)
jjmedina	0:a5367bd4e404	2611	{
jjmedina	0:a5367bd4e404	2612	contadorCeros_X = 0;
jjmedina	0:a5367bd4e404	2613	difX = (Xvibweighted_max - Xvibweighted_min);
jjmedina	0:a5367bd4e404	2614	if ( difX > max_peaktopeak_X_ ){max_peaktopeak_X_ = difX;}
jjmedina	0:a5367bd4e404	2615	//almacena para A95
jjmedina	0:a5367bd4e404	2616	fprintf(fp3,"%f \n", difX);
jjmedina	0:a5367bd4e404	2617	contador_peaktopeak_X++;
jjmedina	0:a5367bd4e404	2618	Xvibweighted_max = 0.0;
jjmedina	0:a5367bd4e404	2619	Xvibweighted_min = 0.0;
jjmedina	0:a5367bd4e404	2620	}
jjmedina	0:a5367bd4e404	2621	}
jjmedina	0:a5367bd4e404	2622	}
jjmedina	0:a5367bd4e404	2623
jjmedina	0:a5367bd4e404	2624	//////////////////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	2625	////////Yvib/////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	2626	//////////////////////////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	2627	//signo
jjmedina	0:a5367bd4e404	2628	signoControl_Y = (int)(Yvibweighted/fabs(Yvibweighted));//+1 positivo, -1 negativo
jjmedina	0:a5367bd4e404	2629	if ( i == Bo1 )
jjmedina	0:a5367bd4e404	2630	{
jjmedina	0:a5367bd4e404	2631	signoCompara_Y = (int)(Yvibweighted/fabs(Yvibweighted));//+1 positivo, -1 negativo
jjmedina	0:a5367bd4e404	2632	}
jjmedina	0:a5367bd4e404	2633	if ( Bo1 == Bo2 )
jjmedina	0:a5367bd4e404	2634	{
jjmedina	0:a5367bd4e404	2635	max_peaktopeak_Y_ = 0.0;
jjmedina	0:a5367bd4e404	2636	}else
jjmedina	0:a5367bd4e404	2637	{
jjmedina	0:a5367bd4e404	2638	if (( Bo1 < i) && (i < Bo2 ))
jjmedina	0:a5367bd4e404	2639	{
jjmedina	0:a5367bd4e404	2640	if ( (signoControl_Y-signoCompara_Y) != 0 )
jjmedina	0:a5367bd4e404	2641	{
jjmedina	0:a5367bd4e404	2642	contadorCeros_Y++;
jjmedina	0:a5367bd4e404	2643	signoCompara_Y = signoControl_Y;
jjmedina	0:a5367bd4e404	2644	}
jjmedina	0:a5367bd4e404	2645	if ( (1 <= contadorCeros_Y) && (contadorCeros_Y < 3) )
jjmedina	0:a5367bd4e404	2646	{
jjmedina	0:a5367bd4e404	2647	if ( Yvibweighted > Yvibweighted_max )
jjmedina	0:a5367bd4e404	2648	{
jjmedina	0:a5367bd4e404	2649	Yvibweighted_max = Yvibweighted;
jjmedina	0:a5367bd4e404	2650	}
jjmedina	0:a5367bd4e404	2651	if ( Yvibweighted < Yvibweighted_min )
jjmedina	0:a5367bd4e404	2652	{
jjmedina	0:a5367bd4e404	2653	Yvibweighted_min = Yvibweighted;
jjmedina	0:a5367bd4e404	2654	}
jjmedina	0:a5367bd4e404	2655	}else if (contadorCeros_Y == 3)
jjmedina	0:a5367bd4e404	2656	{
jjmedina	0:a5367bd4e404	2657	contadorCeros_Y = 0;
jjmedina	0:a5367bd4e404	2658	difY = (Yvibweighted_max - Yvibweighted_min);
jjmedina	0:a5367bd4e404	2659	if ( difY > max_peaktopeak_Y_ ){max_peaktopeak_Y_ = difY;}
jjmedina	0:a5367bd4e404	2660	//almacena para A95
jjmedina	0:a5367bd4e404	2661	fprintf(fp4,"%f \n", difY);
jjmedina	0:a5367bd4e404	2662	contador_peaktopeak_Y++;
jjmedina	0:a5367bd4e404	2663	Yvibweighted_max = 0.0;
jjmedina	0:a5367bd4e404	2664	Yvibweighted_min = 0.0;
jjmedina	0:a5367bd4e404	2665	}
jjmedina	0:a5367bd4e404	2666	}
jjmedina	0:a5367bd4e404	2667	}
jjmedina	0:a5367bd4e404	2668
jjmedina	0:a5367bd4e404	2669	}//fin Loop Principal
jjmedina	0:a5367bd4e404	2670
jjmedina	0:a5367bd4e404	2671
jjmedina	0:a5367bd4e404	2672	fclose(fp);
jjmedina	0:a5367bd4e404	2673	if ( fp2 != NULL )
jjmedina	0:a5367bd4e404	2674	{
jjmedina	0:a5367bd4e404	2675	fclose(fp2);
jjmedina	0:a5367bd4e404	2676	}
jjmedina	0:a5367bd4e404	2677	if ( fp3 != NULL )
jjmedina	0:a5367bd4e404	2678	{
jjmedina	0:a5367bd4e404	2679	fclose(fp3);
jjmedina	0:a5367bd4e404	2680	}
jjmedina	0:a5367bd4e404	2681	if ( fp4 != NULL )
jjmedina	0:a5367bd4e404	2682	{
jjmedina	0:a5367bd4e404	2683	fclose(fp4);
jjmedina	0:a5367bd4e404	2684	}
jjmedina	0:a5367bd4e404	2685
jjmedina	0:a5367bd4e404	2686
jjmedina	0:a5367bd4e404	2687	/////////////////////////////////////////////////////////////
jjmedina	0:a5367bd4e404	2688	//Calculo A95s///////////////////////////////////////////////
jjmedina	0:a5367bd4e404	2689	//Z
jjmedina	0:a5367bd4e404	2690	//abro fichero
jjmedina	0:a5367bd4e404	2691
jjmedina	0:a5367bd4e404	2692	fp2 = fopen(A95file,"r"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95file); }
jjmedina	0:a5367bd4e404	2693	float maxZvibweighted;
jjmedina	0:a5367bd4e404	2694	int length_maximoZvib = (int)(contador_peaktopeak_car*0.05) + 1;
jjmedina	0:a5367bd4e404	2695	int maxindex(0);
jjmedina	0:a5367bd4e404	2696	vector<float> maximoZvib (length_maximoZvib,0.0);
jjmedina	0:a5367bd4e404	2697	vector<float> diferencia_vec;
jjmedina	0:a5367bd4e404	2698	vector<float>::iterator up;
jjmedina	0:a5367bd4e404	2699	float diferencia;
jjmedina	0:a5367bd4e404	2700	float maximo;
jjmedina	0:a5367bd4e404	2701	int contadorA95(0);
jjmedina	0:a5367bd4e404	2702	//pasada 1
jjmedina	0:a5367bd4e404	2703	for(unsigned int i = 0; i<contador_peaktopeak_car; i++)
jjmedina	0:a5367bd4e404	2704	{
jjmedina	0:a5367bd4e404	2705	fscanf(fp2,"%f", &maxZvibweighted);
jjmedina	0:a5367bd4e404	2706	for (unsigned int j = 0; j<length_maximoZvib; j++)
jjmedina	0:a5367bd4e404	2707	{
jjmedina	0:a5367bd4e404	2708	diferencia = maxZvibweighted - maximoZvib.at(j);
jjmedina	0:a5367bd4e404	2709	if ( diferencia < 0 )
jjmedina	0:a5367bd4e404	2710	{
jjmedina	0:a5367bd4e404	2711	diferencia_vec.push_back(0.0);
jjmedina	0:a5367bd4e404	2712	contadorA95++;
jjmedina	0:a5367bd4e404	2713	}else
jjmedina	0:a5367bd4e404	2714	{
jjmedina	0:a5367bd4e404	2715	diferencia_vec.push_back(diferencia);
jjmedina	0:a5367bd4e404	2716	}
jjmedina	0:a5367bd4e404	2717	}
jjmedina	0:a5367bd4e404	2718	if ( contadorA95 == length_maximoZvib ){}
jjmedina	0:a5367bd4e404	2719	else
jjmedina	0:a5367bd4e404	2720	{
jjmedina	0:a5367bd4e404	2721	maximo = *max_element(diferencia_vec.begin(),diferencia_vec.end());
jjmedina	0:a5367bd4e404	2722	up = find(diferencia_vec.begin(), diferencia_vec.end(), maximo);
jjmedina	0:a5367bd4e404	2723	maxindex = int(up - diferencia_vec.begin());
jjmedina	0:a5367bd4e404	2724	maximoZvib.erase(maximoZvib.begin() + maxindex );
jjmedina	0:a5367bd4e404	2725	maximoZvib.insert(maximoZvib.begin() + maxindex ,maxZvibweighted);
jjmedina	0:a5367bd4e404	2726	}
jjmedina	0:a5367bd4e404	2727	contadorA95 = 0;
jjmedina	0:a5367bd4e404	2728	diferencia_vec.clear();
jjmedina	0:a5367bd4e404	2729	}
jjmedina	0:a5367bd4e404	2730	sort(maximoZvib.begin(),maximoZvib.end());//ascending
jjmedina	0:a5367bd4e404	2731	reverse(maximoZvib.begin(),maximoZvib.end());//descending
jjmedina	0:a5367bd4e404	2732	fclose(fp2);
jjmedina	0:a5367bd4e404	2733
jjmedina	0:a5367bd4e404	2734
jjmedina	0:a5367bd4e404	2735
jjmedina	0:a5367bd4e404	2736	//pasada 2
jjmedina	0:a5367bd4e404	2737	//abro fichero
jjmedina	0:a5367bd4e404	2738	fp2 = fopen(A95file,"r"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95file); }
jjmedina	0:a5367bd4e404	2739	double resta(1000.0);
jjmedina	0:a5367bd4e404	2740	for(unsigned int i = 0; i<contador_peaktopeak_car; i++)
jjmedina	0:a5367bd4e404	2741	{
jjmedina	0:a5367bd4e404	2742	fscanf(fp2,"%f", &maxZvibweighted);
jjmedina	0:a5367bd4e404	2743	if( ((maximoZvib.at(maximoZvib.size() - 1) - maxZvibweighted) <= resta) && (maxZvibweighted < maximoZvib.at(maximoZvib.size() - 1)) )
jjmedina	0:a5367bd4e404	2744	{
jjmedina	0:a5367bd4e404	2745	A95_peaktopeak_car_ = maxZvibweighted;
jjmedina	0:a5367bd4e404	2746	resta = maximoZvib.at(maximoZvib.size() - 1) - maxZvibweighted;
jjmedina	0:a5367bd4e404	2747
jjmedina	0:a5367bd4e404	2748	}
jjmedina	0:a5367bd4e404	2749	}
jjmedina	0:a5367bd4e404	2750	//clear all vectors
jjmedina	0:a5367bd4e404	2751	maximoZvib.clear();
jjmedina	0:a5367bd4e404	2752	diferencia_vec.clear();
jjmedina	0:a5367bd4e404	2753
jjmedina	0:a5367bd4e404	2754
jjmedina	0:a5367bd4e404	2755
jjmedina	0:a5367bd4e404	2756	fclose(fp2);
jjmedina	0:a5367bd4e404	2757
jjmedina	0:a5367bd4e404	2758	//X/////////////////////////////////////////
jjmedina	0:a5367bd4e404	2759	//abro fichero
jjmedina	0:a5367bd4e404	2760	fp3 = fopen(A95fileX,"r"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileX); }
jjmedina	0:a5367bd4e404	2761	float maxXvibweighted;
jjmedina	0:a5367bd4e404	2762	int length_maximoXvib = (int)(contador_peaktopeak_X*0.05) + 1;
jjmedina	0:a5367bd4e404	2763	maxindex = 0;
jjmedina	0:a5367bd4e404	2764	vector<float> maximoXvib (length_maximoXvib,0.0);
jjmedina	0:a5367bd4e404	2765	diferencia = 0.0;
jjmedina	0:a5367bd4e404	2766	maximo =0.0;
jjmedina	0:a5367bd4e404	2767	contadorA95 = 0;
jjmedina	0:a5367bd4e404	2768	if ( fp3 == NULL )
jjmedina	0:a5367bd4e404	2769	{
jjmedina	0:a5367bd4e404	2770	A95_peaktopeak_X_ = 0.0;
jjmedina	0:a5367bd4e404	2771	}else
jjmedina	0:a5367bd4e404	2772	{
jjmedina	0:a5367bd4e404	2773	//pasada 1
jjmedina	0:a5367bd4e404	2774	for(unsigned int i = 0; i<contador_peaktopeak_X; i++)
jjmedina	0:a5367bd4e404	2775	{
jjmedina	0:a5367bd4e404	2776	fscanf(fp3,"%f", &maxXvibweighted);
jjmedina	0:a5367bd4e404	2777	for (unsigned int j = 0; j<length_maximoXvib; j++)
jjmedina	0:a5367bd4e404	2778	{
jjmedina	0:a5367bd4e404	2779	diferencia = maxXvibweighted - maximoXvib.at(j);
jjmedina	0:a5367bd4e404	2780	if ( diferencia < 0 )
jjmedina	0:a5367bd4e404	2781	{
jjmedina	0:a5367bd4e404	2782	diferencia_vec.push_back(0.0);
jjmedina	0:a5367bd4e404	2783	contadorA95++;
jjmedina	0:a5367bd4e404	2784	}else
jjmedina	0:a5367bd4e404	2785	{
jjmedina	0:a5367bd4e404	2786	diferencia_vec.push_back(diferencia);
jjmedina	0:a5367bd4e404	2787	}
jjmedina	0:a5367bd4e404	2788	}
jjmedina	0:a5367bd4e404	2789	if ( contadorA95 == length_maximoXvib ){}
jjmedina	0:a5367bd4e404	2790	else
jjmedina	0:a5367bd4e404	2791	{
jjmedina	0:a5367bd4e404	2792	maximo = *max_element(diferencia_vec.begin(),diferencia_vec.end());
jjmedina	0:a5367bd4e404	2793	up = find(diferencia_vec.begin(), diferencia_vec.end(), maximo);
jjmedina	0:a5367bd4e404	2794	maxindex = int(up - diferencia_vec.begin());
jjmedina	0:a5367bd4e404	2795	maximoXvib.erase(maximoXvib.begin() + maxindex );
jjmedina	0:a5367bd4e404	2796	maximoXvib.insert(maximoXvib.begin() + maxindex ,maxXvibweighted);
jjmedina	0:a5367bd4e404	2797	}
jjmedina	0:a5367bd4e404	2798	contadorA95 = 0;
jjmedina	0:a5367bd4e404	2799	diferencia_vec.clear();
jjmedina	0:a5367bd4e404	2800	}
jjmedina	0:a5367bd4e404	2801	sort(maximoXvib.begin(),maximoXvib.end());//ascending
jjmedina	0:a5367bd4e404	2802	reverse(maximoXvib.begin(),maximoXvib.end());//descending
jjmedina	0:a5367bd4e404	2803	fclose(fp3);
jjmedina	0:a5367bd4e404	2804
jjmedina	0:a5367bd4e404	2805	//pasada 2
jjmedina	0:a5367bd4e404	2806	//abro fichero
jjmedina	0:a5367bd4e404	2807	fp3 = fopen(A95fileX,"r"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileX); }
jjmedina	0:a5367bd4e404	2808	resta = 1000.0;
jjmedina	0:a5367bd4e404	2809	for(unsigned int i = 0; i<contador_peaktopeak_X; i++)
jjmedina	0:a5367bd4e404	2810	{
jjmedina	0:a5367bd4e404	2811	fscanf(fp3,"%f", &maxXvibweighted);
jjmedina	0:a5367bd4e404	2812	if( ((maximoXvib.at(maximoXvib.size() - 1) - maxXvibweighted) <= resta) && (maxXvibweighted < maximoXvib.at(maximoXvib.size() - 1)) )
jjmedina	0:a5367bd4e404	2813	{
jjmedina	0:a5367bd4e404	2814	A95_peaktopeak_X_ = maxXvibweighted;
jjmedina	0:a5367bd4e404	2815	resta = maximoXvib.at(maximoXvib.size() - 1) - maxXvibweighted;
jjmedina	0:a5367bd4e404	2816
jjmedina	0:a5367bd4e404	2817	}
jjmedina	0:a5367bd4e404	2818	}
jjmedina	0:a5367bd4e404	2819	//clear all vectors
jjmedina	0:a5367bd4e404	2820	maximoXvib.clear();
jjmedina	0:a5367bd4e404	2821	diferencia_vec.clear();
jjmedina	0:a5367bd4e404	2822	}
jjmedina	0:a5367bd4e404	2823
jjmedina	0:a5367bd4e404	2824
jjmedina	0:a5367bd4e404	2825
jjmedina	0:a5367bd4e404	2826	fclose(fp3);
jjmedina	0:a5367bd4e404	2827
jjmedina	0:a5367bd4e404	2828	//Y/////////////////////////////////////////
jjmedina	0:a5367bd4e404	2829	//abro fichero
jjmedina	0:a5367bd4e404	2830	fp4 = fopen(A95fileY,"r"); if(fp4 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileY); }
jjmedina	0:a5367bd4e404	2831	float maxYvibweighted;
jjmedina	0:a5367bd4e404	2832	int length_maximoYvib = (int)(contador_peaktopeak_Y*0.05) + 1;
jjmedina	0:a5367bd4e404	2833	maxindex = 0;
jjmedina	0:a5367bd4e404	2834	vector<float> maximoYvib (length_maximoYvib,0.0);
jjmedina	0:a5367bd4e404	2835	diferencia = 0.0;
jjmedina	0:a5367bd4e404	2836	maximo =0.0;
jjmedina	0:a5367bd4e404	2837	contadorA95 = 0;
jjmedina	0:a5367bd4e404	2838	if ( fp4 == NULL )
jjmedina	0:a5367bd4e404	2839	{
jjmedina	0:a5367bd4e404	2840	A95_peaktopeak_Y_ = 0.0;
jjmedina	0:a5367bd4e404	2841	}else
jjmedina	0:a5367bd4e404	2842	{
jjmedina	0:a5367bd4e404	2843	//pasada 1
jjmedina	0:a5367bd4e404	2844	for(unsigned int i = 0; i<contador_peaktopeak_Y; i++)
jjmedina	0:a5367bd4e404	2845	{
jjmedina	0:a5367bd4e404	2846	fscanf(fp4,"%f", &maxYvibweighted);
jjmedina	0:a5367bd4e404	2847	for (unsigned int j = 0; j<length_maximoYvib; j++)
jjmedina	0:a5367bd4e404	2848	{
jjmedina	0:a5367bd4e404	2849	diferencia = maxYvibweighted - maximoYvib.at(j);
jjmedina	0:a5367bd4e404	2850	if ( diferencia < 0 )
jjmedina	0:a5367bd4e404	2851	{
jjmedina	0:a5367bd4e404	2852	diferencia_vec.push_back(0.0);
jjmedina	0:a5367bd4e404	2853	contadorA95++;
jjmedina	0:a5367bd4e404	2854	}else
jjmedina	0:a5367bd4e404	2855	{
jjmedina	0:a5367bd4e404	2856	diferencia_vec.push_back(diferencia);
jjmedina	0:a5367bd4e404	2857	}
jjmedina	0:a5367bd4e404	2858	}
jjmedina	0:a5367bd4e404	2859	if ( contadorA95 == length_maximoYvib ){}
jjmedina	0:a5367bd4e404	2860	else
jjmedina	0:a5367bd4e404	2861	{
jjmedina	0:a5367bd4e404	2862	maximo = *max_element(diferencia_vec.begin(),diferencia_vec.end());
jjmedina	0:a5367bd4e404	2863	up = find(diferencia_vec.begin(), diferencia_vec.end(), maximo);
jjmedina	0:a5367bd4e404	2864	maxindex = int(up - diferencia_vec.begin());
jjmedina	0:a5367bd4e404	2865	maximoYvib.erase(maximoYvib.begin() + maxindex );
jjmedina	0:a5367bd4e404	2866	maximoYvib.insert(maximoYvib.begin() + maxindex ,maxYvibweighted);
jjmedina	0:a5367bd4e404	2867	}
jjmedina	0:a5367bd4e404	2868	contadorA95 = 0;
jjmedina	0:a5367bd4e404	2869	diferencia_vec.clear();
jjmedina	0:a5367bd4e404	2870	}
jjmedina	0:a5367bd4e404	2871	sort(maximoYvib.begin(),maximoYvib.end());//ascending
jjmedina	0:a5367bd4e404	2872	reverse(maximoYvib.begin(),maximoYvib.end());//descending
jjmedina	0:a5367bd4e404	2873	fclose(fp4);
jjmedina	0:a5367bd4e404	2874
jjmedina	0:a5367bd4e404	2875	//pasada 2
jjmedina	0:a5367bd4e404	2876	//abro fichero
jjmedina	0:a5367bd4e404	2877	fp4 = fopen(A95fileY,"r"); if(fp4 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileY); }
jjmedina	0:a5367bd4e404	2878	resta = 1000.0;
jjmedina	0:a5367bd4e404	2879	for(unsigned int i = 0; i<contador_peaktopeak_Y; i++)
jjmedina	0:a5367bd4e404	2880	{
jjmedina	0:a5367bd4e404	2881	fscanf(fp4,"%f", &maxYvibweighted);
jjmedina	0:a5367bd4e404	2882	if( ((maximoYvib.at(maximoYvib.size() - 1) - maxYvibweighted) <= resta) && (maxYvibweighted < maximoYvib.at(maximoYvib.size() - 1)) )
jjmedina	0:a5367bd4e404	2883	{
jjmedina	0:a5367bd4e404	2884	A95_peaktopeak_Y_ = maxYvibweighted;
jjmedina	0:a5367bd4e404	2885	resta = maximoYvib.at(maximoYvib.size() - 1) - maxYvibweighted;
jjmedina	0:a5367bd4e404	2886
jjmedina	0:a5367bd4e404	2887	}
jjmedina	0:a5367bd4e404	2888	}
jjmedina	0:a5367bd4e404	2889	//clear all vectors
jjmedina	0:a5367bd4e404	2890	maximoYvib.clear();
jjmedina	0:a5367bd4e404	2891	diferencia_vec.clear();
jjmedina	0:a5367bd4e404	2892	}
jjmedina	0:a5367bd4e404	2893
jjmedina	0:a5367bd4e404	2894
jjmedina	0:a5367bd4e404	2895	fclose(fp4);
jjmedina	0:a5367bd4e404	2896	//remove files
jjmedina	0:a5367bd4e404	2897	int rmDVA0 = remove(A95file); if(rmDVA0 != 0) { display.Clear();display.SetXY(0,0); display.printf("Error removing %s file\n", A95file); }
jjmedina	0:a5367bd4e404	2898
jjmedina	0:a5367bd4e404	2899	int rmDVA1 = remove(A95fileX); if(rmDVA1 != 0) { display.Clear();display.SetXY(0,0); display.printf("Error removing %s file\n", A95fileX); }
jjmedina	0:a5367bd4e404	2900	int rmDVA2 = remove(A95fileY); if(rmDVA2 != 0) { display.Clear();display.SetXY(0,0); display.printf("Error removing %s file\n", A95fileY); }
jjmedina	0:a5367bd4e404	2901	int rmDVA3 = remove(dispVelFile); if(rmDVA3 != 0) { display.Clear();display.SetXY(0,0); display.printf("Error removing %s file\n", dispVelFile); }
jjmedina	0:a5367bd4e404	2902
jjmedina	0:a5367bd4e404	2903
jjmedina	0:a5367bd4e404	2904
jjmedina	0:a5367bd4e404	2905
jjmedina	0:a5367bd4e404	2906
jjmedina	0:a5367bd4e404	2907
jjmedina	0:a5367bd4e404	2908
jjmedina	0:a5367bd4e404	2909
jjmedina	0:a5367bd4e404	2910	return;
jjmedina	0:a5367bd4e404	2911
jjmedina	0:a5367bd4e404	2912	}
jjmedina	0:a5367bd4e404	2913
jjmedina	0:a5367bd4e404	2914	float RegresionJerk(float * aZ, unsigned int ndat)
jjmedina	0:a5367bd4e404	2915	{
jjmedina	0:a5367bd4e404	2916	//Media de las aceleraciones
jjmedina	0:a5367bd4e404	2917	float aZm = 0.0;
jjmedina	0:a5367bd4e404	2918	for(unsigned int j = 0; j<ndat; j++) { aZm += aZ[j]; }
jjmedina	0:a5367bd4e404	2919	aZm = aZm/(float)ndat;
jjmedina	0:a5367bd4e404	2920
jjmedina	0:a5367bd4e404	2921	//Pendiente por minimos cuadrados
jjmedina	0:a5367bd4e404	2922	float b;
jjmedina	0:a5367bd4e404	2923	//Numerador y denominador en expresion de pendiente
jjmedina	0:a5367bd4e404	2924	float numer = 0.0;
jjmedina	0:a5367bd4e404	2925	float denom = 0.0;
jjmedina	0:a5367bd4e404	2926
jjmedina	0:a5367bd4e404	2927	int nn;
jjmedina	0:a5367bd4e404	2928	float At;
jjmedina	0:a5367bd4e404	2929
jjmedina	0:a5367bd4e404	2930	for(int j = 0; j<ndat; j++)
jjmedina	0:a5367bd4e404	2931	{
jjmedina	0:a5367bd4e404	2932	nn = j-((ndat/2));
jjmedina	0:a5367bd4e404	2933	At = (float)nn(1.0e-6)(float)(PasoTiempo);
jjmedina	0:a5367bd4e404	2934	denom = denom + At*At;
jjmedina	0:a5367bd4e404	2935	numer = numer + At*(aZ[j]-aZm);
jjmedina	0:a5367bd4e404	2936	}
jjmedina	0:a5367bd4e404	2937	b = numer/denom;
jjmedina	0:a5367bd4e404	2938
jjmedina	0:a5367bd4e404	2939	return b;
jjmedina	0:a5367bd4e404	2940	}
jjmedina	0:a5367bd4e404	2941
jjmedina	0:a5367bd4e404	2942	void DisplayResultados1(void)
jjmedina	0:a5367bd4e404	2943	{
jjmedina	0:a5367bd4e404	2944	for(int i = 0; i<2; i++)
jjmedina	0:a5367bd4e404	2945	{
jjmedina	0:a5367bd4e404	2946
jjmedina	0:a5367bd4e404	2947	display.Clear();
jjmedina	0:a5367bd4e404	2948	if(TRADUCE==0)
jjmedina	0:a5367bd4e404	2949	{
jjmedina	0:a5367bd4e404	2950	display.SetXY(0,0);
jjmedina	0:a5367bd4e404	2951	display.printf("Datos velocidad");
jjmedina	0:a5367bd4e404	2952	display.SetXY(0,1);
jjmedina	0:a5367bd4e404	2953	display.printf("aceleracion jerk");
jjmedina	0:a5367bd4e404	2954	}else
jjmedina	0:a5367bd4e404	2955	{
jjmedina	0:a5367bd4e404	2956	display.SetXY(0,0);
jjmedina	0:a5367bd4e404	2957	display.printf("Velocity jerk");
jjmedina	0:a5367bd4e404	2958	display.SetXY(0,1);
jjmedina	0:a5367bd4e404	2959	display.printf("acceleration");
jjmedina	0:a5367bd4e404	2960	}
jjmedina	0:a5367bd4e404	2961
jjmedina	0:a5367bd4e404	2962	wait(2.0);
jjmedina	0:a5367bd4e404	2963
jjmedina	0:a5367bd4e404	2964	display.Clear();
jjmedina	0:a5367bd4e404	2965	display.SetXY(0,0);
jjmedina	0:a5367bd4e404	2966	display.printf("VNom = %4.2f m/s \n",vNom);
jjmedina	0:a5367bd4e404	2967	display.SetXY(0,1);
jjmedina	0:a5367bd4e404	2968	display.printf("Vmax = %4.2f m/s \n", Vmax);
jjmedina	0:a5367bd4e404	2969
jjmedina	0:a5367bd4e404	2970	wait(2.0);
jjmedina	0:a5367bd4e404	2971
jjmedina	0:a5367bd4e404	2972	display.Clear();
jjmedina	0:a5367bd4e404	2973	display.SetXY(0,0);
jjmedina	0:a5367bd4e404	2974	display.printf("amax = %4.2f m/s2\n", amax);
jjmedina	0:a5367bd4e404	2975	display.SetXY(0,1);
jjmedina	0:a5367bd4e404	2976	display.printf("dmax = %4.2f m/s2\n", dmax);
jjmedina	0:a5367bd4e404	2977
jjmedina	0:a5367bd4e404	2978	wait(2.0);
jjmedina	0:a5367bd4e404	2979
jjmedina	0:a5367bd4e404	2980	display.Clear();
jjmedina	0:a5367bd4e404	2981	display.SetXY(0,0);
jjmedina	0:a5367bd4e404	2982	display.printf("jmax = %4.2f m/s3\n", jerkmax);
jjmedina	0:a5367bd4e404	2983	display.SetXY(0,1);
jjmedina	0:a5367bd4e404	2984	display.printf("VA95 = %4.2f m/s \n", VA95);
jjmedina	0:a5367bd4e404	2985
jjmedina	0:a5367bd4e404	2986	wait(2.0);
jjmedina	0:a5367bd4e404	2987
jjmedina	0:a5367bd4e404	2988	display.Clear();
jjmedina	0:a5367bd4e404	2989	display.SetXY(0,0);
jjmedina	0:a5367bd4e404	2990	display.printf("aA95 = %4.2f m/s2\n", aA95);
jjmedina	0:a5367bd4e404	2991	display.SetXY(0,1);
jjmedina	0:a5367bd4e404	2992	display.printf("dA95 = %4.2f m/s2\n", dA95);
jjmedina	0:a5367bd4e404	2993
jjmedina	0:a5367bd4e404	2994	wait(2.0);
jjmedina	0:a5367bd4e404	2995
jjmedina	0:a5367bd4e404	2996	}
jjmedina	0:a5367bd4e404	2997	}
jjmedina	0:a5367bd4e404	2998
jjmedina	0:a5367bd4e404	2999	void DisplayResultados2(void)
jjmedina	0:a5367bd4e404	3000	{
jjmedina	0:a5367bd4e404	3001	for(int i = 0; i<2; i++)
jjmedina	0:a5367bd4e404	3002	{
jjmedina	0:a5367bd4e404	3003
jjmedina	0:a5367bd4e404	3004	display.Clear();
jjmedina	0:a5367bd4e404	3005	if(TRADUCE==0)
jjmedina	0:a5367bd4e404	3006	{
jjmedina	0:a5367bd4e404	3007	display.SetXY(0,0);
jjmedina	0:a5367bd4e404	3008	display.printf("Datos confort");
jjmedina	0:a5367bd4e404	3009	display.SetXY(0,0);
jjmedina	0:a5367bd4e404	3010	display.printf("vibracion Z,X,Y:");
jjmedina	0:a5367bd4e404	3011	}else
jjmedina	0:a5367bd4e404	3012	{
jjmedina	0:a5367bd4e404	3013	display.SetXY(0,0);
jjmedina	0:a5367bd4e404	3014	display.printf("Comfort info");
jjmedina	0:a5367bd4e404	3015	display.SetXY(0,0);
jjmedina	0:a5367bd4e404	3016	display.printf("vibration Z,X,Y:");
jjmedina	0:a5367bd4e404	3017	}
jjmedina	0:a5367bd4e404	3018	wait(2.0);
jjmedina	0:a5367bd4e404	3019
jjmedina	0:a5367bd4e404	3020
jjmedina	0:a5367bd4e404	3021
jjmedina	0:a5367bd4e404	3022	display.Clear();
jjmedina	0:a5367bd4e404	3023	display.SetXY(0,0);
jjmedina	0:a5367bd4e404	3024	display.printf("Zmaxca=%4.2f m/s2 \n", max_peaktopeak_car_);
jjmedina	0:a5367bd4e404	3025	display.SetXY(0,1);
jjmedina	0:a5367bd4e404	3026	display.printf("ZA95ca=%4.2f m/s2 \n", A95_peaktopeak_car_);
jjmedina	0:a5367bd4e404	3027
jjmedina	0:a5367bd4e404	3028	wait(2.0);
jjmedina	0:a5367bd4e404	3029
jjmedina	0:a5367bd4e404	3030	display.Clear();
jjmedina	0:a5367bd4e404	3031	display.SetXY(0,0);
jjmedina	0:a5367bd4e404	3032	display.printf("Zmaxnca=%4.2f m/s2 \n", max_peaktopeak_ncar_);
jjmedina	0:a5367bd4e404	3033
jjmedina	0:a5367bd4e404	3034	wait(2.0);
jjmedina	0:a5367bd4e404	3035
jjmedina	0:a5367bd4e404	3036	display.Clear();
jjmedina	0:a5367bd4e404	3037	display.SetXY(0,0);
jjmedina	0:a5367bd4e404	3038	display.printf("Xmax = %4.2f m/s2\n", max_peaktopeak_X_);
jjmedina	0:a5367bd4e404	3039	display.SetXY(0,1);
jjmedina	0:a5367bd4e404	3040	display.printf("XA95 = %4.2f m/s2\n", A95_peaktopeak_X_);
jjmedina	0:a5367bd4e404	3041
jjmedina	0:a5367bd4e404	3042	wait(2.0);
jjmedina	0:a5367bd4e404	3043
jjmedina	0:a5367bd4e404	3044	display.Clear();
jjmedina	0:a5367bd4e404	3045	display.SetXY(0,0);
jjmedina	0:a5367bd4e404	3046	display.printf("Ymax = %4.2f m/s2\n", max_peaktopeak_Y_);
jjmedina	0:a5367bd4e404	3047	display.SetXY(0,1);
jjmedina	0:a5367bd4e404	3048	display.printf("YA95 = %4.2f m/s2\n", A95_peaktopeak_Y_);
jjmedina	0:a5367bd4e404	3049
jjmedina	0:a5367bd4e404	3050	wait(2.0);
jjmedina	0:a5367bd4e404	3051	}
jjmedina	0:a5367bd4e404	3052	}
jjmedina	0:a5367bd4e404	3053
jjmedina	0:a5367bd4e404	3054	void FileResultados( unsigned int ContadorEnsayos )
jjmedina	0:a5367bd4e404	3055	{
jjmedina	0:a5367bd4e404	3056	FILE * fp;
jjmedina	0:a5367bd4e404	3057
jjmedina	0:a5367bd4e404	3058	char NoViaje[2];
jjmedina	0:a5367bd4e404	3059	char resultsFile[50];
jjmedina	0:a5367bd4e404	3060
jjmedina	0:a5367bd4e404	3061	strcpy(resultsFile,"/local/ISO_");
jjmedina	0:a5367bd4e404	3062
jjmedina	0:a5367bd4e404	3063	sprintf (NoViaje, "%d", ContadorEnsayos);
jjmedina	0:a5367bd4e404	3064	strcat(resultsFile,NoViaje);
jjmedina	0:a5367bd4e404	3065	strcat(resultsFile,".txt");
jjmedina	0:a5367bd4e404	3066
jjmedina	0:a5367bd4e404	3067
jjmedina	0:a5367bd4e404	3068	fp = fopen(resultsFile,"w"); if(fp == NULL) { display.printf("Could not open file %s for read\n", resultsFile); }
jjmedina	0:a5367bd4e404	3069	fprintf(fp,"%f %f %f %f %f %f %f %f %f %f %f %f %f %f \n", amax, dmax, aA95, dA95, jerkmax, Vmax, VA95, max_peaktopeak_car_, A95_peaktopeak_car_, max_peaktopeak_ncar_, max_peaktopeak_X_, A95_peaktopeak_X_, max_peaktopeak_Y_, A95_peaktopeak_Y_);
jjmedina	0:a5367bd4e404	3070	//fprintf(fp,"%d %d %d \n", Bo1, Bo2, Bo3);
jjmedina	0:a5367bd4e404	3071	fclose(fp);
jjmedina	0:a5367bd4e404	3072
jjmedina	0:a5367bd4e404	3073	}
jjmedina	0:a5367bd4e404	3074
jjmedina	0:a5367bd4e404	3075	void LeeAcelerometroBuffer()
jjmedina	0:a5367bd4e404	3076	{
jjmedina	0:a5367bd4e404	3077	bma180.ReadAccels_BMA180(readings);
jjmedina	0:a5367bd4e404	3078
jjmedina	0:a5367bd4e404	3079	aaX = (int16_t)readings[0];
jjmedina	0:a5367bd4e404	3080	aaY = (int16_t)readings[1];
jjmedina	0:a5367bd4e404	3081	aaZ = (int16_t)readings[2];
jjmedina	0:a5367bd4e404	3082	//tt_inst = (int16_t)timer.read_us();
jjmedina	0:a5367bd4e404	3083
jjmedina	0:a5367bd4e404	3084	aX = (-1.0)((float)aaX-(-103.23))0.0018;
jjmedina	0:a5367bd4e404	3085	aY = (-1.0)((float)aaY-(57.21))0.0018;
jjmedina	0:a5367bd4e404	3086	aZ = (-1.0)((float)aaZ-(701.1184))0.0018097570403;
jjmedina	0:a5367bd4e404	3087
jjmedina	0:a5367bd4e404	3088	//t_inst = (float)tt_inst;
jjmedina	0:a5367bd4e404	3089
jjmedina	0:a5367bd4e404	3090	//buff_t.put(t_inst);
jjmedina	0:a5367bd4e404	3091	buff_aX.put(aX);
jjmedina	0:a5367bd4e404	3092	buff_aY.put(aY);
jjmedina	0:a5367bd4e404	3093	buff_aZ.put(aZ);
jjmedina	0:a5367bd4e404	3094	}
jjmedina	0:a5367bd4e404	3095
jjmedina	0:a5367bd4e404	3096	void GuardaDatosBuffer()
jjmedina	0:a5367bd4e404	3097	{
jjmedina	0:a5367bd4e404	3098	bool empty = buff_aX.isEmpty();
jjmedina	0:a5367bd4e404	3099	int wi = buff_aX.getWritingIndex();
jjmedina	0:a5367bd4e404	3100	int ri = buff_aX.getReadingIndex();
jjmedina	0:a5367bd4e404	3101
jjmedina	0:a5367bd4e404	3102	int dif; //Diferencia entre writting index y riding index. El buffer se llena cuando esta diferencia alcanza el tamaño del buffer
jjmedina	0:a5367bd4e404	3103	float tiempo; //Tiempo en segundos
jjmedina	0:a5367bd4e404	3104
jjmedina	0:a5367bd4e404	3105	if((wi-ri)>0) { dif = wi-ri; }
jjmedina	0:a5367bd4e404	3106	else { dif = sizeBuff+wi-ri; }
jjmedina	0:a5367bd4e404	3107
jjmedina	0:a5367bd4e404	3108	if(empty == false)
jjmedina	0:a5367bd4e404	3109	{
jjmedina	0:a5367bd4e404	3110	aXr = buff_aX.get();
jjmedina	0:a5367bd4e404	3111	aYr = buff_aY.get();
jjmedina	0:a5367bd4e404	3112	aZr = buff_aZ.get();
jjmedina	0:a5367bd4e404	3113
jjmedina	0:a5367bd4e404	3114	tiempo = ((float)ContadorDatos)*((float)PasoTiempo)/1.0e6;
jjmedina	0:a5367bd4e404	3115
jjmedina	0:a5367bd4e404	3116	//pc.printf("%d %f %f %f %d\n", ContadorDatos*PasoTiempo, aXr, aYr, aZr, dif);
jjmedina	0:a5367bd4e404	3117	fprintf(fp,"%f %f %f %f %d\n", tiempo, aXr, aYr, aZr, dif);
jjmedina	0:a5367bd4e404	3118	ContadorDatos++;
jjmedina	0:a5367bd4e404	3119	}
jjmedina	0:a5367bd4e404	3120	}
jjmedina	0:a5367bd4e404	3121
jjmedina	0:a5367bd4e404	3122	void ParamDependFrecAdq()
jjmedina	0:a5367bd4e404	3123	{
jjmedina	0:a5367bd4e404	3124
jjmedina	0:a5367bd4e404	3125	if(FrecAdq == 320)
jjmedina	0:a5367bd4e404	3126	{
jjmedina	0:a5367bd4e404	3127	PasoTiempo = 3125;
jjmedina	0:a5367bd4e404	3128	gain = 1.184456202e+02;
jjmedina	0:a5367bd4e404	3129	c_butt1 = -0.7575469445;
jjmedina	0:a5367bd4e404	3130	c_butt2 = 1.7237761728;
jjmedina	0:a5367bd4e404	3131	ndes = 100;
jjmedina	0:a5367bd4e404	3132	nmed = 180;
jjmedina	0:a5367bd4e404	3133	}
jjmedina	0:a5367bd4e404	3134
jjmedina	0:a5367bd4e404	3135	if(FrecAdq == 1000)
jjmedina	0:a5367bd4e404	3136	{
jjmedina	0:a5367bd4e404	3137	PasoTiempo = 1000;
jjmedina	0:a5367bd4e404	3138	gain = 1.058546241e+03;
jjmedina	0:a5367bd4e404	3139	c_butt1 = -0.9149758348;
jjmedina	0:a5367bd4e404	3140	c_butt2 = 1.9111970674;
jjmedina	0:a5367bd4e404	3141	ndes = 300;
jjmedina	0:a5367bd4e404	3142	nmed = 600;
jjmedina	0:a5367bd4e404	3143	}
jjmedina	0:a5367bd4e404	3144
jjmedina	0:a5367bd4e404	3145	/// Para fadq = 100 Hz, PasoTiempo = 10000 ///
jjmedina	0:a5367bd4e404	3146
jjmedina	0:a5367bd4e404	3147	//float gain = 1.482463775e+01;
jjmedina	0:a5367bd4e404	3148	//float c_butt1 = -0.4128015981;
jjmedina	0:a5367bd4e404	3149	//float c_butt2 = 1.1429805025;
jjmedina	0:a5367bd4e404	3150	//unsigned int ndes = 30;
jjmedina	0:a5367bd4e404	3151	//unsigned int nmed = 60;
jjmedina	0:a5367bd4e404	3152
jjmedina	0:a5367bd4e404	3153	/// Para fadq = 328 Hz, PasoTiempo = 3050 ///
jjmedina	0:a5367bd4e404	3154
jjmedina	0:a5367bd4e404	3155	//float gain = 1.239670776e+02;
jjmedina	0:a5367bd4e404	3156	//float c_butt1 = -0.7626121550;
jjmedina	0:a5367bd4e404	3157	//float c_butt2 = 1.7303455236;
jjmedina	0:a5367bd4e404	3158	//unsigned int ndes = 100;
jjmedina	0:a5367bd4e404	3159	//unsigned int nmed = 180;
jjmedina	0:a5367bd4e404	3160
jjmedina	0:a5367bd4e404	3161	/// Para fadq = 320 Hz, PasoTiempo = 3125 ///
jjmedina	0:a5367bd4e404	3162
jjmedina	0:a5367bd4e404	3163	//const float gain = 1.184456202e+02;
jjmedina	0:a5367bd4e404	3164	//const float c_butt1 = -0.7575469445;
jjmedina	0:a5367bd4e404	3165	//const float c_butt2 = 1.7237761728;
jjmedina	0:a5367bd4e404	3166	//unsigned int ndes = 100;
jjmedina	0:a5367bd4e404	3167	//unsigned int nmed = 180;
jjmedina	0:a5367bd4e404	3168
jjmedina	0:a5367bd4e404	3169	/// Para fadq = 1000 Hz, PasoTiempo = 1000 ///
jjmedina	0:a5367bd4e404	3170
jjmedina	0:a5367bd4e404	3171	//const float gain = 1.058546241e+03;
jjmedina	0:a5367bd4e404	3172	//const float c_butt1 = -0.9149758348;
jjmedina	0:a5367bd4e404	3173	//const float c_butt2 = 1.9111970674;
jjmedina	0:a5367bd4e404	3174	//const unsigned int ndes = 300;
jjmedina	0:a5367bd4e404	3175	//const unsigned int nmed = 600;
jjmedina	0:a5367bd4e404	3176	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	20 May 2014
Imports:	7
Forks:	0
Commits:	3
Dependents:	0
Dependencies:	6
Followers:	8

vmConfort.cpp@2:cc4a43d806e2, 2014-06-06 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning