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

Dependencies:   ADXL345 Display1602 MSCFileSystem SDFileSystem mbed FATFileSystem

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?

UserRevisionLine numberNew 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&#65533;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] + fa*2.0*h + fab*(-2.0*h*h) + fabc*(-4.0*h*h*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] + fa*2.0*h + fab*(-2.0*h*h) + fabc*(-4.0*h*h*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.5*running_time*1.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] + fa*2.0*h + fab*(-2.0*h*h) + fabc*(-4.0*h*h*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&#65533;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.5*running_time*1.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 ( ((timeStep*regiones.at(2*i)-factorT)<0) || ((timeStep*regiones.at(2*i +1)+factorT)>time.at(time.size()-1)) )
jjmedina 0:a5367bd4e404 2362 while ( ((timeStep*regiones.at(2*i)-factorT)<0) || ((timeStep*regiones.at(2*i +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] = timeStep*regiones.at(2*i)-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] = timeStep*regiones.at(2*i +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