virtualmech
Esta versión v6 pasa a ser el nuevo master. Funciona correctamente
Dependencies: ADXL345 Display1602 MSCFileSystem SDFileSystem mbed FATFileSystem
Embed:
(wiki syntax)
Show/hide line numbers
vmConfort.cpp
00001 #include "mbed.h" 00002 #include "Display1602.h" 00003 //#include "SDFileSystem.h" 00004 #include "MSCFileSystem.h" 00005 00006 #include <iostream> 00007 #include <math.h> 00008 #include <stdio.h> 00009 #include <string.h> 00010 #include <list> 00011 #include <vector> 00012 #include <algorithm> 00013 00014 #include "filters.h" 00015 #include "BMA180.h" 00016 //#include "ADXL345.h" 00017 #include "RingBuffer.h" 00018 00019 using namespace std; 00020 00021 00022 Timer timer; 00023 00024 int TiempoInicio = 0; 00025 int TiempoFin = 0; 00026 00027 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00028 /// CONSTANTES DEPENDIENTES DE LA FRECUENCIA DE ADQUISICION (sus valores se establecen en ParamDependFrecAdq()) /// 00029 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00030 00031 unsigned int PasoTiempo; //Paso de tiempo de adquisicion en [us]. Frecuencia de adquisicion = 1/PasoTiemo 00032 float gain; // CONSTANTE DEL FILTRO DE BUTTERWORTH PASO BAJO 10 HZ 00033 float c_butt1; // CONSTANTE DEL FILTRO DE BUTTERWORTH PASO BAJO 10 HZ 00034 float c_butt2; // CONSTANTE DEL FILTRO DE BUTTERWORTH PASO BAJO 10 HZ 00035 unsigned int ndes; //Puntos que se descartan al principio por el Filtro Butterworth 00036 unsigned int nmed; //Puntos para calcular la media de aZ al principio del viaje despues de los puntos descartados 00037 00038 int nDatosPorPaso = 1; 00039 00040 ///////////////////////////////// 00041 /// DISPOSITIVOS, PERIFERICOS /// 00042 ///////////////////////////////// 00043 00044 /// ACELEROMETRO ANALOGICO /// 00045 00046 //AnalogIn Pin_aX(p15); 00047 //AnalogIn Pin_aY(p16); 00048 //AnalogIn Pin_aZ(p17); 00049 00050 /// ACELEROMETRO DIGITAL Bosch/// 00051 00052 BMA180 bma180(p5,p6,p7,p8); 00053 00054 /// ACELEROMETRO DIGITAL AnalogDevices ADXL345/// 00055 00056 //ADXL345 adxl345(p5, p6, p7, p8); 00057 00058 Serial pc(USBTX, USBRX); // tx, rx 00059 00060 /// TARJETA SD /// 00061 00062 //SDFileSystem sd(p11, p12, p13, p14, "local"); 00063 00064 /// PEN DRIVE /// 00065 00066 MSCFileSystem msc("local"); 00067 00068 /// MEMORIA FLASH LOCAL DE LA MBED /// 00069 00070 LocalFileSystem local("mbedFlash"); // Create the local filesystem under the name "local" 00071 00072 /// LEDS /// 00073 00074 DigitalOut red_LED(p30); 00075 DigitalOut green_LED(p29); 00076 00077 /// PUSH BUTTONS /// 00078 00079 DigitalOut red_PUSH(p9); 00080 DigitalOut green_PUSH(p10); 00081 00082 /// DISPLAY LCD /// 00083 00084 //Display1602 display(p18,p19,p20,p28,p27,p26,p25,p24,p23,p22,p21); 00085 Display1602 display(p18,p19,p20,p21,p22,p23,p24,p25,p26,p27,p28); 00086 00087 //void InterpolaMedidasPerdidas(unsigned int, unsigned int); 00088 void FiltradoPeriodosIntegracion(unsigned int, unsigned int); 00089 void DatosConfort(unsigned int, unsigned int, int); 00090 float RegresionJerk(float *, unsigned int); 00091 void calcula_n05n95_Boveloc_jerkmax(unsigned int , unsigned int, int ); 00092 void calcula_A95_V_acel_decel(unsigned int, unsigned int, int); 00093 void vibraReadFilter(unsigned int , unsigned int ); 00094 00095 void calculaCarNCar(unsigned int , unsigned int, int ); 00096 void calculaStep2(unsigned int , unsigned int ); 00097 00098 void calculaZXYvibweighted( unsigned int , unsigned int); 00099 00100 void DisplayResultados1(void); 00101 00102 void DisplayResultados2(void); 00103 00104 void FileResultados( unsigned int ); 00105 00106 unsigned int nArr, nPar, nV0, nVfin; //Instantes (orden en el vector de datos) caracteristicos del viaje 00107 float t_Arr, t_V0, t_Vfin, t_Par; //Instantes (valores del tiempo) caracteristicos del viaje 00108 float vArr = 0.0; //Velocidad que se calcula por integración en el instante de arranque (debe ser 0) 00109 float vPar = 0.0; //Velocidad que se calcula por integración en el instante de parada (debe ser 0) 00110 00111 float vZmFin = 0.0; //Media de vZ al final del viaje (debe ser zero pero no lo es) 00112 float tFin; //Instante final 00113 00114 float aZm0 = 0.0; //Aceleracion media de referencia en instantes iniciales en dirección vertical Z 00115 float aYm0 = 0.0; //Aceleracion media de referencia en instantes iniciales en dirección lateral X 00116 float aXm0 = 0.0; //Aceleracion media de referencia en instantes iniciales en dirección lateral Y 00117 00118 ////////////////////////// 00119 /// MEDIDAS EN DISPLAY /// 00120 ////////////////////////// 00121 00122 float vNom = 0.0; //Velocidad nominal 00123 float amax = 0.0; 00124 float dmax = 0.0; 00125 float Vmax = 0.0; 00126 float jerkmax = 0.0; 00127 float VA95(0.0); 00128 float aA95(0.0); 00129 float dA95(0.0); 00130 00131 //por pantalla 00132 float max_peaktopeak_car_(0.0); 00133 float A95_peaktopeak_car_(0.0); 00134 float max_peaktopeak_ncar_(0.0); 00135 float max_peaktopeak_X_(0.0); 00136 float A95_peaktopeak_X_(0.0); 00137 float max_peaktopeak_Y_(0.0); 00138 float A95_peaktopeak_Y_(0.0); 00139 00140 float d_arr, d_par; //Desplazamientos medidos en instantes de arranque y de parada 00141 00142 unsigned int Bo0, Bo1, Bo2, Bo3; //Limites de los periodos de calculo de norma ISO 00143 unsigned int Boacel; //Instante donde deja de acelerar para empezar a decelerar 00144 00145 float Vmax1 = 0.0; 00146 float Vmax2 = 0.0; 00147 unsigned int n05Vmax1(0);unsigned int n95Vmax1(0);unsigned int n05Vmax2(0);unsigned int n95Vmax2(0); 00148 unsigned int Bo_veloc_acel(0); 00149 unsigned int Bo_veloc_decel(0); 00150 00151 int Nregionesconst(0); 00152 int Nregiones(0); 00153 float **region; 00154 float regionconst[20][4]; //50 porque no sabemos el numero de filas. RESOLVER MEJOR, JUAN 04-05-2012. 00155 00156 //////////////////////// 00157 /// IDIOMA DE LA GUI /// 00158 //////////////////////// 00159 00160 unsigned int TRADUCE = 0; // TRADUCE = 0 gui en español, TRADUCE = 1, traduce gui al inglés 00161 unsigned int SeleccionIdioma = 0; // Vale 1 cuando ya se ha seleccionado el idioma 00162 00163 ///////////////////////////////// 00164 /// FRECUENCIA DE ADQUISICION /// 00165 ///////////////////////////////// 00166 00167 unsigned int FrecAdq = 0; //La frecuencia de adquisición puede ser 320 Hz (FrecAdq = 320) o 1000 Hz (FrecAdq = 1000) 00168 unsigned int SeleccionFrecAdq = 0; // Vale 1 cuando ya se ha seleccionado la frecuencia de adquisicion 00169 00170 00171 ////////////////////////////////////////// 00172 /// ALMACENAMIENTO DE DATOS CON BUFFER /// 00173 ////////////////////////////////////////// 00174 00175 void LeeAcelerometroBuffer(); 00176 void GuardaDatosBuffer(); 00177 00178 int readings[3] = {0, 0, 0}; //Lectura aceleraciones sensor digital 00179 float aX, aY, aZ; //Medidas de aceleracion 00180 int16_t aaX, aaY, aaZ; //Medidas de aceleracion y tiempo 00181 00182 float aXr, aYr, aZr; //Tiempo y medidas de aceleracion leidos del buffer 00183 unsigned int contDatos = 0; //Contador de datos 00184 00185 int sizeBuff = 500; //Tamaño de los buffer que guardan los datos 00186 00187 Ticker tickerLectura; 00188 00189 Buffer buff_aX(sizeBuff); 00190 Buffer buff_aY(sizeBuff); 00191 Buffer buff_aZ(sizeBuff); 00192 00193 FILE * fp; 00194 FILE * fp2; 00195 00196 unsigned int ContadorEnsayos = 0; 00197 unsigned int ContadorDatos = 0; 00198 unsigned int ContadorMedidas = 0; 00199 00200 void ParamDependFrecAdq(void); 00201 00202 int main() { 00203 pc.baud(115200); 00204 00205 timer.start(); 00206 00207 bool datosPC_green = false; //true = envia datos a PC, false = Guarda datos en tarjeta SD 00208 bool ensayo_red = false; //true = Comienza la medida, false = termina la medida 00209 00210 char NoViaje[2]; 00211 00212 ////////////////////////////////////////////// 00213 /// ESTABLECE LOS OFFSETS DEL ACELEROMETRO /// 00214 ////////////////////////////////////////////// 00215 00216 char offsets[6]; 00217 00218 offsets[0]= 0xAE; //x_msb 00219 offsets[1]= 0x9E; //y_msb 00220 offsets[2]= 0x95; //z_msb 00221 00222 offsets[3]= 0x0C; //x_lsb 00223 offsets[4]= 0x04; //y_lsb 00224 offsets[5]= 0x0C; //z_lsb 00225 00226 00227 //bma180.AcelerometroLeeOffsets(readings); 00228 //pc.printf("%d %d %d\n", readings[0], readings[1], readings[2]); 00229 //bma180.AcelerometroEscribeOffsets(offsets); 00230 //bma180.AcelerometroLeeOffsets(readings); 00231 //pc.printf("%d %d %d\n", readings[0], readings[1], readings[2]); 00232 00233 ///////////////////////////// 00234 /// APAGA EL ACELEROMETRO /// 00235 ///////////////////////////// 00236 00237 bma180.AcelerometroSleep(); 00238 00239 ///////////////////////////////////// 00240 /// SELECCION DE IDIOMA DE LA GUI /// 00241 ///////////////////////////////////// 00242 00243 display.Clear(); display.SetXY(0,0); display.printf("English: yellow"); display.SetXY(0,1); display.printf("Castellano: rojo"); 00244 00245 while(SeleccionIdioma == 0) 00246 { 00247 if(red_PUSH.read() == 1) 00248 { 00249 TRADUCE = 0;//traduce gui al español 00250 SeleccionIdioma = 1; 00251 wait(0.5); 00252 } 00253 if(green_PUSH.read() == 1) 00254 { 00255 TRADUCE = 1;//traduce gui al ingles 00256 SeleccionIdioma = 1; 00257 wait(0.5); 00258 } 00259 } 00260 00261 ///////////////////////////////////////////////// 00262 /// SELECCION DE LA FRECUENCIA DE ADQUISICION /// 00263 ///////////////////////////////////////////////// 00264 00265 if(TRADUCE == 0) 00266 { 00267 display.Clear(); display.SetXY(0,0); display.printf("320 Hz: amarillo"); display.SetXY(0,1); display.printf("1000 Hz: rojo"); 00268 } 00269 if(TRADUCE == 1) 00270 { 00271 display.Clear(); display.SetXY(0,0); display.printf("320 Hz: yellow"); display.SetXY(0,1); display.printf("1000 Hz: red"); 00272 } 00273 00274 while(SeleccionFrecAdq == 0) 00275 { 00276 if(red_PUSH.read() == 1) 00277 { 00278 FrecAdq = 1000; 00279 SeleccionFrecAdq = 1; 00280 wait(0.5); 00281 } 00282 if(green_PUSH.read() == 1) 00283 { 00284 FrecAdq = 320; 00285 SeleccionFrecAdq = 1; 00286 wait(0.5); 00287 } 00288 } 00289 00290 ParamDependFrecAdq(); //Establece el valor de los parámetros que depeneden de la frecuencia de adquisicion 00291 00292 //////////////////////////////////////////////// 00293 /// AVISA MODO FUNCIONAMIENTO: PC O AUTONOMO /// 00294 //////////////////////////////////////////////// 00295 00296 if(datosPC_green == true) 00297 { 00298 green_LED = 1; 00299 if(TRADUCE == 0) 00300 { 00301 display.Clear(); display.SetXY(0,0); display.printf("Aparato en"); display.SetXY(0,1); display.printf("modo PC."); 00302 wait(1.5); 00303 display.Clear(); display.SetXY(0,0); display.printf("Para cambio a"); display.SetXY(0,1); display.printf("modo autonomo..."); 00304 wait(1.5); 00305 display.Clear(); display.SetXY(0,0); display.printf("...pulse boton"); display.SetXY(0,1); display.printf("amarillo."); 00306 wait(1.5); 00307 display.Clear(); 00308 } 00309 else 00310 { 00311 display.Clear(); display.SetXY(0,0); display.printf("Set in PC mode."); 00312 wait(1.5); 00313 display.Clear(); display.SetXY(0,0); display.printf("To change to"); display.SetXY(0,1); display.printf("autonomous mode"); 00314 wait(1.5); 00315 display.Clear(); display.SetXY(0,0); display.printf("...press"); display.SetXY(0,1); display.printf("yellow button."); 00316 wait(1.5); 00317 display.Clear(); 00318 } 00319 } 00320 else 00321 { 00322 green_LED = 0; 00323 00324 if(TRADUCE == 0) 00325 { 00326 display.Clear(); display.SetXY(0,0); display.printf("Aparato en"); display.SetXY(0,1); display.printf("modo autonomo."); 00327 wait(1.5); 00328 display.Clear(); display.SetXY(0,0); display.printf("Para cambio a"); display.SetXY(0,1); display.printf("modo PC..."); 00329 wait(1.5); 00330 display.Clear(); display.SetXY(0,0); display.printf("...pulse boton"); display.SetXY(0,1); display.printf("amarillo."); 00331 wait(1.5); 00332 display.Clear(); 00333 } 00334 else 00335 { 00336 display.Clear(); display.SetXY(0,0); display.printf("Set in "); display.SetXY(0,1); display.printf("autonomous mode."); 00337 wait(1.5); 00338 display.Clear(); display.SetXY(0,0); display.printf("To change to"); display.SetXY(0,1); display.printf("PC mode..."); 00339 wait(1.5); 00340 display.Clear(); display.SetXY(0,0); display.printf("...press"); display.SetXY(0,1); display.printf("yellow button."); 00341 wait(1.5); 00342 display.Clear(); 00343 } 00344 } 00345 00346 00347 00348 while((SeleccionIdioma == 1)&&(SeleccionFrecAdq == 1)) 00349 { 00350 00351 //////////////////////////////////////// 00352 /// INVITA A COMENZAR LA ADQUISICION /// 00353 //////////////////////////////////////// 00354 00355 if(ensayo_red == false) 00356 { 00357 if(TRADUCE == 0) 00358 { 00359 display.Clear(); 00360 display.SetXY(0,0); display.printf("Pulse rojo"); 00361 display.SetXY(0,1); display.printf("para empezar"); 00362 wait(0.1); 00363 } 00364 if(TRADUCE == 1) 00365 { 00366 display.Clear(); 00367 display.SetXY(0,0); display.printf("Push red"); 00368 display.SetXY(0,1); display.printf("to start"); 00369 wait(0.1); 00370 } 00371 } 00372 /////////////////////////////// 00373 // COMIENZO Y FIN DE MEDIDDA // 00374 /////////////////////////////// 00375 00376 if(red_PUSH.read() == 1) //si pulsa el rojo (o empieza o acaba medidas.) 00377 { 00378 if(ensayo_red == false)//antes de pulsar estaba apagado, por tanto empieza medidas. 00379 { 00380 ensayo_red = true;//lo enciende 00381 00382 //////////////////////////////// 00383 /// ENCIENDE EL ACELEROMETRO /// 00384 //////////////////////////////// 00385 00386 bma180.AcelerometroWakeUp(); 00387 00388 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) 00389 { 00390 // LEE DE FICHERO EL ORDEN DEL ENSAYO ANTERIOR Y VUELVE A GUARDAR EL ORDEN INCREMENTADO EN 1 00391 fp2 = fopen("/mbedFlash/OrdEns.txt","r"); 00392 if(fp2 == NULL) 00393 { 00394 //SI NO EXISTE EL ARCHIVO CON EL ORDEN DEL ENSAYO SE CREA Y SE PONE UN 0 00395 pc.printf("Could not open file OrdEns.txt for read\n"); 00396 fp2 = fopen("/mbedFlash/OrdEns.txt","w"); 00397 if(fp2 == NULL) { pc.printf("Could not open file OrdEns.txt for write\n"); } 00398 fprintf(fp2,"0"); 00399 fclose(fp2); 00400 } 00401 fscanf(fp2,"%d", &ContadorEnsayos); 00402 ContadorEnsayos++; 00403 00404 if(TRADUCE==0) 00405 { 00406 display.Clear(); display.SetXY(0,0); display.printf("Comienza medida"); display.SetXY(0,1); display.printf("Viaje %d ...", ContadorEnsayos); 00407 } 00408 else 00409 { 00410 display.Clear(); display.SetXY(0,0); display.printf("Measuring ride"); display.SetXY(0,1); display.printf("number %d ...", ContadorEnsayos); 00411 } 00412 00413 00414 if(ContadorEnsayos == 100) { ContadorEnsayos = 0; } //EL NUMERO MAXIMO DE ENSAYOS ES 100. SI SE SUPERA SE PONE EL CONTADOR A 0 00415 pc.printf("Ensayo: %d\n",ContadorEnsayos); 00416 fclose(fp2); 00417 fp2 = fopen("/mbedFlash/OrdEns.txt","w"); 00418 if(fp2 == NULL) { pc.printf("Could not open file OrdEns.txt for write\n"); } 00419 fprintf(fp2,"%d", ContadorEnsayos); 00420 fclose(fp2); 00421 00422 ContadorDatos = 0; 00423 ContadorMedidas = 0; 00424 //Crea y abre un nuevo archivo para guardar aceleraciones 00425 char resultsFile[50]; 00426 00427 strcpy(resultsFile,"/local/raw_V_"); 00428 00429 int n = sprintf (NoViaje, "%d", ContadorEnsayos); 00430 strcat(resultsFile,NoViaje); 00431 strcat(resultsFile,".txt"); 00432 00433 msc.disk_initialize(); 00434 00435 fp = fopen(resultsFile,"w"); 00436 00437 if(fp == NULL) 00438 { 00439 if(TRADUCE==0) 00440 { 00441 display.Clear();display.SetXY(0,0); display.printf("USB desconectado");display.SetXY(0,1); display.printf("o lleno"); 00442 } 00443 else 00444 { 00445 display.Clear();display.SetXY(0,0); display.printf("USB disconnected");display.SetXY(0,1); display.printf("or full"); 00446 } 00447 /////////////////////////////////////////////////////////////////////////////////////////// 00448 /// LO DE ABAJO ES PARA QUE NO SE QUEDE COLGADO SI HEMOS OLVIDADO CONECTAR EL PEN-DRIVE /// 00449 /////////////////////////////////////////////////////////////////////////////////////////// 00450 ensayo_red = false; 00451 ContadorEnsayos--; 00452 fp2 = fopen("/mbedFlash/OrdEns.txt","w"); 00453 if(fp2 == NULL) { pc.printf("Could not open file OrdEns.txt for write\n"); } 00454 fprintf(fp2,"%d", ContadorEnsayos); 00455 fclose(fp2); 00456 } 00457 00458 ///////////////////////////////////////////////////////////////////// 00459 /// OJO: COMIENZA EL TICKER DE LECTURA DE DATOS DEL ACELERÓMETRO /// 00460 ///////////////////////////////////////////////////////////////////// 00461 if(ensayo_red == true) 00462 { 00463 tickerLectura.attach_us(&LeeAcelerometroBuffer, PasoTiempo); 00464 } 00465 } 00466 00467 wait(0.5); 00468 } 00469 else //antes de pulsar estaba encendido 00470 { 00471 ensayo_red = false; // lo apaga, es decir acaba medidas y procesa datos si en modo autonomo 00472 if(ensayo_red == true) { red_LED = 1;} else { red_LED = 0;}//enciende o apaga el LED rojo 00473 00474 pc.printf("-25717505\n"); //Señal para vmRideanalizer 00475 00476 ///////////////////////////// 00477 /// APAGA EL ACELEROMETRO /// 00478 ///////////////////////////// 00479 00480 bma180.AcelerometroSleep(); 00481 00482 if(datosPC_green == false)//procesa datos si en modo autonomo. Cuando entra aquí, estan LEDs apagados + ensayo_red = false + datosPC_green == false 00483 { 00484 /////////////////////////////////////////// 00485 // POSTPROCESO DE DATOS EN MODO AUTONOMO //Primera tanda 00486 /////////////////////////////////////////// 00487 00488 ///////////////////////////////////////////////////////////////////// 00489 /// OJO: FINALIZA EL TICKER DE LECTURA DE DATOS DEL ACELERÓMETRO /// 00490 ///////////////////////////////////////////////////////////////////// 00491 00492 tickerLectura.detach(); 00493 00494 fclose(fp);//cierra fichero con datos en el USB 00495 00496 //if(TRADUCE==0) 00497 //{ 00498 // display.Clear(); 00499 // display.SetXY(0,0); display.printf("Interpola datos"); 00500 //}else 00501 //{ 00502 // display.Clear(); 00503 // display.SetXY(0,0); display.printf("Interpolating data"); 00504 // display.SetXY(0,1); display.printf("data"); 00505 //} 00506 //InterpolaMedidasPerdidas(ContadorEnsayos, ContadorDatos); 00507 00508 if(TRADUCE==0) 00509 { 00510 display.Clear(); display.SetXY(0,0); display.printf("Rojo: posprocesa"); display.SetXY(0,1); display.printf("Amarillo: fin"); 00511 }else 00512 { 00513 display.Clear(); display.SetXY(0,0); display.printf("Red: postprocess"); display.SetXY(0,1); display.printf("Yellow: finalize"); 00514 } 00515 00516 wait(0.5); 00517 00518 while(1) 00519 { 00520 /////////////////////////////////////////// 00521 // POSTPROCESO DE DATOS EN MODO AUTONOMO //Primera tanda 00522 /////////////////////////////////////////// 00523 //loop infinito hasta que pulsa el rojo otra vez para ver mas resultados o el verde para terminar y grabar resultados en fichero 00524 if(red_PUSH.read() == 1) //si pulsa el rojo 00525 { 00526 //ponemos variables globales a zero 00527 //aceleraciones medias 00528 aZm0 = 0.0; 00529 aYm0 = 0.0; 00530 aXm0 = 0.0; 00531 //veloc truco Dani 00532 vZmFin = 0.0; 00533 vNom = 0.0; 00534 //maximos ISO 00535 amax = 0.0; 00536 dmax = 0.0; 00537 Vmax = 0.0; 00538 jerkmax = 0.0; 00539 max_peaktopeak_car_ = 0.0; 00540 max_peaktopeak_ncar_ = 0.0; 00541 max_peaktopeak_X_ = 0.0; 00542 max_peaktopeak_Y_ = 0.0; 00543 Vmax1 = 0.0; 00544 Vmax2 = 0.0; 00545 00546 if(TRADUCE==0) 00547 { 00548 display.Clear(); display.SetXY(0,0); display.printf("Comienza el"); display.SetXY(0,1); display.printf("postproceso ..."); 00549 } 00550 else 00551 { 00552 display.Clear(); display.SetXY(0,0); display.printf("Starting"); display.SetXY(0,1); display.printf("postprocess ..."); 00553 } 00554 00555 //InterpolaMedidasPerdidas(ContadorEnsayos, ContadorDatos); 00556 00557 FiltradoPeriodosIntegracion(ContadorEnsayos, ContadorDatos); 00558 00559 //display.Clear(); display.SetXY(0,0); display.printf("Interpolacion,"); display.SetXY(0,1); display.printf("filtro, integracion"); 00560 00561 DatosConfort(ContadorEnsayos, ContadorDatos, PasoTiempo); 00562 00563 //display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("Vel. nom. y jerk"); 00564 00565 calcula_n05n95_Boveloc_jerkmax(ContadorEnsayos, ContadorDatos, PasoTiempo); 00566 00567 //ha terminado de calcular amax, dmax, Vmax y jerkmax 00568 00569 //display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("A95s"); 00570 00571 calcula_A95_V_acel_decel(ContadorEnsayos, ContadorDatos, PasoTiempo); 00572 00573 //ha terminado de calcular A95 de a, d y V. Muestra por pantalla 00574 00575 DisplayResultados1(); 00576 00577 00578 if(FrecAdq == 320) 00579 { 00580 if(TRADUCE==0) 00581 { 00582 display.Clear(); display.SetXY(0,0); display.printf("Rojo continua"); display.SetXY(0,1); display.printf("Amarillo fin"); 00583 }else 00584 { 00585 display.Clear(); display.SetXY(0,0); display.printf("Red continue"); display.SetXY(0,1); display.printf("Yellow finalize"); 00586 } 00587 } 00588 break;//ha calculado resultados restantes y finaliza 00589 }else if(green_PUSH.read() == 1) 00590 { 00591 break; 00592 } 00593 }//primera tanda resultados 00594 00595 00596 00597 while(FrecAdq == 320) 00598 { 00599 /////////////////////////////////////////// 00600 // POSTPROCESO DE DATOS EN MODO AUTONOMO //Segunda tanda 00601 /////////////////////////////////////////// 00602 00603 //loop infinito hasta que pulsa el rojo otra vez para ver mas resultados o el verde para terminar y grabar resultados en fichero 00604 00605 if(red_PUSH.read() == 1) //si pulsa el rojo 00606 { 00607 if(TRADUCE==0) 00608 { 00609 display.Clear(); display.SetXY(0,0); display.printf("Cont. Posproceso"); display.SetXY(0,1); display.printf("vibracion Z,X,Y"); 00610 }else 00611 { 00612 display.Clear(); display.SetXY(0,0); display.printf("Con. Postprocess"); display.SetXY(0,1); display.printf("vibration Z,X,Y"); 00613 } 00614 //empieza cálculos de ZXYvibweighted 00615 vibraReadFilter(ContadorEnsayos, ContadorDatos); 00616 00617 //display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("step2"); 00618 00619 calculaStep2( ContadorEnsayos, ContadorDatos); 00620 00621 //display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("car-ncar"); 00622 00623 calculaCarNCar( ContadorEnsayos, ContadorDatos, PasoTiempo ); //pc.printf("Termina 'calculaCarNCar'\n"); 00624 00625 //display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("maxZXY"); 00626 00627 calculaZXYvibweighted( ContadorEnsayos, ContadorDatos ); 00628 00629 //display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("Display"); 00630 00631 DisplayResultados2(); 00632 00633 break;//ha calculado resultados restantes y finaliza 00634 }else if(green_PUSH.read() == 1) 00635 { 00636 break; 00637 } 00638 } 00639 00640 //segunda tanda resultados 00641 //copia resultados en un fichero y finaliza 00642 if(TRADUCE==0) 00643 { 00644 display.Clear(); display.SetXY(0,0); display.printf("Guarda resultados"); display.SetXY(0,1); display.printf("en fichero"); 00645 }else 00646 { 00647 display.Clear(); display.SetXY(0,0); display.printf("Save results"); display.SetXY(0,1); display.printf("in file"); 00648 } 00649 00650 FileResultados( ContadorEnsayos ); 00651 00652 wait(0.5); 00653 00654 if(TRADUCE==0) 00655 { 00656 display.Clear(); display.SetXY(0,0); display.printf("Postproceso"); display.SetXY(0,1); display.printf("fin"); 00657 }else 00658 { 00659 display.Clear(); display.SetXY(0,0); display.printf("Postprocess"); display.SetXY(0,1); display.printf("end"); 00660 } 00661 } 00662 wait(0.5); 00663 display.Clear(); 00664 } 00665 } 00666 00667 if(ensayo_red == true) { red_LED = 1;} else { red_LED = 0;}//enciende o apaga el LED rojo 00668 00669 ///////////////////////////// 00670 ///MODOS DE FUNCIONAMIENTO:// 00671 // AUTONOMO O PC //////////// 00672 ///////////////////////////// 00673 00674 if(ensayo_red == false) 00675 { 00676 if(green_PUSH.read() == 1) 00677 { 00678 datosPC_green = !datosPC_green; 00679 00680 if(datosPC_green == true) 00681 { 00682 if(TRADUCE == 0) 00683 { 00684 display.Clear(); display.SetXY(0,0); display.printf("Aparato en"); display.SetXY(0,1); display.printf("modo PC."); 00685 wait(1.5); 00686 display.Clear(); 00687 } 00688 else 00689 { 00690 display.Clear(); display.SetXY(0,0); display.printf("Set in PC mode."); 00691 wait(1.5); 00692 display.Clear(); 00693 } 00694 } 00695 00696 if(datosPC_green == false) 00697 { 00698 if(TRADUCE == 0) 00699 { 00700 display.Clear(); display.SetXY(0,0); display.printf("Aparato en"); display.SetXY(0,1); display.printf("modo autonomo."); 00701 wait(1.5); 00702 display.Clear(); 00703 } 00704 else 00705 { 00706 display.Clear(); display.SetXY(0,0); display.printf("Set in"); display.SetXY(0,1); display.printf("autonomous mode."); 00707 wait(1.5); 00708 display.Clear(); 00709 } 00710 } 00711 } 00712 00713 if(datosPC_green == true) 00714 { 00715 green_LED = 1; 00716 } 00717 else 00718 { 00719 green_LED = 0; 00720 }//enciende o apaga LED verde 00721 } 00722 00723 ///////////////////////////// 00724 // MEDIDA DE ACELERACIONES // 00725 ///////////////////////////// 00726 00727 if(ensayo_red == true) 00728 { 00729 00730 if(datosPC_green == true)//Manda los datos al PC, puerto serie 00731 { 00732 //////////////////////////////////////////////// 00733 /// LECTURA CON ACELEROMETRO BMA180, DIGITAL /// 00734 //////////////////////////////////////////////// 00735 00736 bma180.ReadAccels_BMA180(readings); 00737 00738 int16_t aaX = (int16_t)readings[0]; 00739 int16_t aaY = (int16_t)readings[1]; 00740 int16_t aaZ = (int16_t)readings[2]; 00741 00742 aX = (-1.0)*((float)aaX-(-103.23))*0.0018; 00743 aY = (-1.0)*((float)aaY-(57.21))*0.0018; 00744 aZ = (-1.0)*((float)aaZ-(701.1184))*0.0018097570403; 00745 00746 ContadorMedidas++; 00747 00748 pc.printf("%d,%f,%f,%f\n", timer.read_us(), aX, aY, aZ); 00749 } 00750 00751 00752 if(datosPC_green == false)//Manda los datos a fichero USB y promedia medidas perdidas 00753 { 00754 00755 GuardaDatosBuffer(); 00756 00757 /* 00758 //////////////////////////////////// 00759 // PROMEDIO MEDIDAS ACELERACIONES // 00760 //////////////////////////////////// 00761 00762 if(ContadorMedidas == 1) {TiempoInicio = timer.read_us();} 00763 00764 TiempoFin = timer.read_us(); 00765 00766 if((TiempoFin-TiempoInicio)<((ContadorDatos+1)*PasoTiempo)) 00767 { 00768 nDatosPorPaso++; 00769 aXm += aX; aYm += aY; aZm += aZ; 00770 } 00771 else 00772 { 00773 //Calculo del numero de pasos en blanco 00774 int PasosEnBlanco = ((TiempoFin-TiempoInicio)-(ContadorDatos+1)*PasoTiempo)/(PasoTiempo); 00775 00776 if(PasosEnBlanco == 0) 00777 { 00778 fprintf(fp,"%d %f %f %f %d\n", ContadorDatos*PasoTiempo, aXm/(float)nDatosPorPaso, aYm/(float)nDatosPorPaso, aZm/(float)nDatosPorPaso, nDatosPorPaso); 00779 aXm = aX; aYm = aY; aZm = aZ; 00780 nDatosPorPaso = 1; 00781 ContadorDatos ++; 00782 } 00783 else 00784 { 00785 for(int i = 0; i<PasosEnBlanco; i++) 00786 { 00787 fprintf(fp,"%d %f %f %f %d\n", (ContadorDatos+i)*PasoTiempo, -20.0, -20.0, -20.0, 0); 00788 //fprintf(fp,"t = %d, PasosEnblanco = %d, FinalMedida = %d \n", (ContadorDatos+i)*PasoTiempo, PasosEnBlanco,TiempoFin-TiempoInicio); 00789 00790 aXm = aX; aYm = aY; aZm = aZ; 00791 } 00792 nDatosPorPaso = 1; 00793 ContadorDatos = ContadorDatos + PasosEnBlanco; 00794 } 00795 }*/ 00796 } 00797 } 00798 } 00799 } 00800 00801 00802 void InterpolaMedidasPerdidas(unsigned int ContadorEnsayos, unsigned int ContadorDatos) 00803 { 00804 int t; 00805 float aXYZ[3]; 00806 float aXYZantes[3]; 00807 float aXYZdespues[3]; 00808 float aXYZinterpolada[3]; 00809 int tPasosPerdidos[300]; 00810 00811 FILE * fp; 00812 FILE * fp2; 00813 00814 char NoViajel[2]; 00815 char resultsFile[50]; char interpolatedFile[50]; 00816 00817 strcpy(resultsFile,"/local/raw_V_"); strcpy(interpolatedFile,"/local/IV_"); 00818 00819 sprintf (NoViajel, "%d", ContadorEnsayos); 00820 strcat(resultsFile,NoViajel); strcat(interpolatedFile,NoViajel); 00821 strcat(resultsFile,".txt"); strcat(interpolatedFile,".dat"); 00822 00823 fp = fopen(resultsFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", resultsFile); } 00824 fp2 = fopen(interpolatedFile,"w"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", interpolatedFile); } 00825 00826 unsigned int i = 0; 00827 unsigned int nDatosPorPaso; 00828 unsigned int PasosPerdidos = 0; 00829 00830 while(i<ContadorDatos) 00831 { 00832 fscanf(fp,"%d %f %f %f %d", &t, &aXYZ[0], &aXYZ[1], &aXYZ[2], &nDatosPorPaso); 00833 00834 00835 00836 if(nDatosPorPaso >0) 00837 { 00838 aXYZantes[0] = aXYZ[0]; aXYZantes[1] = aXYZ[1]; aXYZantes[2] = aXYZ[2]; 00839 fprintf(fp2,"%f %f %f %f\n", (float)t/1000000.0, aXYZ[0], aXYZ[1], aXYZ[2]); 00840 i++; 00841 } 00842 else 00843 { 00844 tPasosPerdidos[PasosPerdidos] = t; 00845 PasosPerdidos = 1; 00846 00847 while(nDatosPorPaso == 0) 00848 { 00849 fscanf(fp,"%d %f %f %f %d", &t, &aXYZ[0], &aXYZ[1], &aXYZ[2], &nDatosPorPaso); 00850 00851 if(nDatosPorPaso == 0) 00852 { 00853 tPasosPerdidos[PasosPerdidos] = t; 00854 PasosPerdidos++; 00855 } 00856 else 00857 { 00858 aXYZdespues[0] = aXYZ[0]; aXYZdespues[1] = aXYZ[1]; aXYZdespues[2] = aXYZ[2]; 00859 } 00860 } 00861 00862 for(unsigned int j = 0; j<PasosPerdidos; j++) 00863 { 00864 //aXYZinterpolada[0] = aXYZantes[0]+((aXYZdespues[0]-aXYZantes[0])/(float(PasosPerdidos+1)))*(float)(j+1); 00865 //aXYZinterpolada[1] = aXYZantes[1]+((aXYZdespues[1]-aXYZantes[1])/(float(PasosPerdidos+1)))*(float)(j+1); 00866 //aXYZinterpolada[2] = aXYZantes[2]+((aXYZdespues[2]-aXYZantes[2])/(float(PasosPerdidos+1)))*(float)(j+1); 00867 00868 aXYZinterpolada[0] = (aXYZdespues[0]+aXYZantes[0])/2.0; 00869 aXYZinterpolada[1] = (aXYZdespues[1]+aXYZantes[1])/2.0; 00870 aXYZinterpolada[2] = (aXYZdespues[2]+aXYZantes[2])/2.0; 00871 00872 fprintf(fp2,"%f %f %f %f\n", (float)tPasosPerdidos[j]/1000000.0, aXYZinterpolada[0], aXYZinterpolada[1], aXYZinterpolada[2]); 00873 i++; 00874 } 00875 fprintf(fp2,"%f %f %f %f\n", (float)t/1000000.0, aXYZ[0], aXYZ[1], aXYZ[2]); 00876 i++; 00877 PasosPerdidos = 0; 00878 } 00879 } 00880 00881 fclose(fp); 00882 fclose(fp2); 00883 00884 //elimina fichero RAW 00885 int rm = remove(resultsFile); 00886 } 00887 00888 00889 void FiltradoPeriodosIntegracion(unsigned int ContadorEnsayos, unsigned int ContadorDatos) 00890 { 00891 float t; 00892 float aXYZ[3]; //vector aceleracion en el paso i [aX,aY,aZ] 00893 00894 float xv[3]; //Para hacer las operaciones del Filtro Butterworth 00895 float yv[3]; //Para hacer las operaciones del Filtro Butterworth 00896 00897 const float tol = 0.05; //Porcentaje en que la aceleracion debe superar la media para considerar que cambia el periodo del viaje 00898 00899 bool mov = false; //Indica si hay o no movimiento 00900 bool velV = false; //Indica si estamos en periodo de velocidad constante 00901 bool decel = false; //Indica si ha empezado la deceleracion 00902 bool fin = false; //Indica si ha terminado el viaje 00903 //float t_Arr, t_V0, t_Vfin, t_Par; //Instantes en que empiezan arranque, ... 00904 00905 float xZ = 0.0; //Posici�n vertical calculada integrando vZ 00906 float vZ = 0.0; //Velocidad vertical calculada integrando la aceleraciOn filtrada menos su media 00907 00908 float t_3[3], aZ_3[3], vZ_3[3], xZ_3[3]; //Datos guardados para hacer integracion numerica de aceleraciones 00909 float xa, xb, xc, h, fa, fb, fc, fab, fbc, fabc; 00910 00911 00912 unsigned int nmedV = 50; //Puntos para calcular la media de vZ al final del viaje (que debe ser zero pero no lo es) 00913 00914 float sgnAcelArr = 0.0; //Signo de la aceleracion vertical de arranque 00915 float sgnAcelFr = 0.0; //Signo de la aceleracion vertical de frenada 00916 float sgnAcelv0 = 0.0; //Signo de la aceleracion vertical al comienzo del periodo de vel cte 00917 float sgnAcelFin = 0.0; //Signo de la aceleracion vertical al final del periodo de vel cte 00918 00919 const float tiempoMinimoArranque = 0.15; //Tiempo mínimo que puede durar la fase de aceleracion de la cabina 00920 const float tiempoMinimoVelCte = 0.15; //Tiempo mínimo que puede durar el periodo de velocidad constante 00921 const float tiempoMinimoFrenada = 0.15; //Tiempo mínimo que puede durar el periodo de deceleracion de la cabina 00922 00923 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;} 00924 00925 FILE * fp; 00926 00927 FILE * fp3; 00928 00929 char NoViajel[2]; 00930 char resultsFile[50]; 00931 char dispVelFile[50]; 00932 00933 int dif; 00934 00935 //strcpy(resultsFile,"/local/IV_"); strcpy(dispVelFile,"/local/dva0_"); 00936 00937 strcpy(resultsFile,"/local/raw_V_"); strcpy(dispVelFile,"/local/dva0_"); 00938 00939 sprintf (NoViajel, "%d", ContadorEnsayos); 00940 00941 strcat(resultsFile,NoViajel); strcat(dispVelFile,NoViajel); 00942 strcat(resultsFile,".txt"); strcat(dispVelFile,".txt"); 00943 00944 //fichero con todos los datos de la funcion anterior IV_ 00945 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); } 00946 //fichero donde guardaron 00947 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); } 00948 00949 //Lee, filtra y guarda las tres aceleraciones por separado 00950 00951 for(unsigned int i = 0; i<ContadorDatos; i++) 00952 { 00953 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()) 00954 00955 /////////////////////////////////////////////////////// 00956 /// FILTRO DE BUTTERWORTH, PASO BAJO, ORDEN 2, 10Hz /// 00957 /////////////////////////////////////////////////////// 00958 00959 if(i < 2) 00960 { 00961 xv[i+1] = aXYZ[2]; 00962 yv[i+1] = aXYZ[2]; 00963 } 00964 else 00965 { 00966 xv[0] = xv[1]; xv[1] = xv[2]; 00967 xv[2] = aXYZ[2] / gain; 00968 00969 yv[0] = yv[1]; yv[1] = yv[2]; 00970 yv[2] = (xv[0] + xv[2]) + 2.0 * xv[1] 00971 + ( c_butt1 * yv[0]) + ( c_butt2 * yv[1]); 00972 aXYZ[2] = yv[2]; 00973 } 00974 00975 ///////////////////////////////////////////////////////////////////////// 00976 /// DETECCION INSTANTES DE ARRANQUE, COMIENZO Vcte, FIN Vcte Y PARADA /// 00977 ///////////////////////////////////////////////////////////////////////// 00978 00979 /// CALCULO DE ACELERACION Z DE REFERENCIA (DEBE COINCIDIR CON LA GRAVEDAD: 9.81) aZm0 00980 00981 if( (i >= ndes)&&(i < (ndes+nmed))) 00982 { 00983 aXm0 += aXYZ[0]/(float)nmed; 00984 aYm0 += aXYZ[1]/(float)nmed; 00985 aZm0 += aXYZ[2]/(float)nmed; 00986 } 00987 //Calculo nArr,nV0,nVfin,nPar 00988 if( (i >= (ndes+nmed))) 00989 { 00990 if ((mov == false) && (fin == false)) 00991 { 00992 if(fabs(aXYZ[2]-aZm0) > fabs(tol*aZm0)) 00993 { 00994 nArr = i-(ndes+nmed); //Detecta el instante de arranque 00995 mov = true; 00996 t_Arr = t; 00997 00998 sgnAcelArr = (aXYZ[2]-aZm0)/fabs(aXYZ[2]-aZm0); //Calcula el signo de la aceleracion vertical de arranque 00999 01000 //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); 01001 } 01002 } 01003 01004 if ( (mov == true) && (velV == false) && (decel == false) && (t-t_Arr>tiempoMinimoArranque) ) 01005 { 01006 if(fabs(aXYZ[2]-aZm0) < fabs(tol*aZm0)) 01007 { 01008 sgnAcelv0 = (aXYZ[2]-aZm0)/fabs(aXYZ[2]-aZm0); 01009 if(fabs(sgnAcelv0-sgnAcelArr)>1.5) //El signo de la aceleracion de arranque tiene que ser distinto del signo de la de frenada 01010 { 01011 nV0 = i-(ndes+nmed); //Detecta el instante de inicio del periodo de V cte 01012 velV = true; 01013 t_V0 = t; 01014 //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); 01015 } 01016 } 01017 } 01018 01019 if ( (mov == true) && (velV == true) && (decel == false) && (t-t_V0>tiempoMinimoVelCte)) 01020 { 01021 if(fabs(aXYZ[2]-aZm0) > fabs(tol*aZm0)) 01022 { 01023 sgnAcelFr = (aXYZ[2]-aZm0)/fabs(aXYZ[2]-aZm0); 01024 01025 if(fabs(sgnAcelFr-sgnAcelArr)>1.5) //El signo de la aceleracion de arranque tiene que ser distinto del signo de la de frenada 01026 { 01027 nVfin = i-(ndes+nmed); //Detecta el instante de fin del periodo de V cte 01028 velV = false; 01029 decel = true; 01030 01031 t_Vfin = t; 01032 01033 //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); 01034 } 01035 } 01036 } 01037 01038 if ( (mov == true) && (velV == false) && (decel == true) && (t-t_Vfin>tiempoMinimoFrenada)) 01039 { 01040 if(fabs(aXYZ[2]-aZm0) < fabs(tol*aZm0)) 01041 { 01042 sgnAcelFin = (aXYZ[2]-aZm0)/fabs(aXYZ[2]-aZm0); 01043 01044 if(fabs(sgnAcelFin-sgnAcelFr)>1.5) 01045 { 01046 nPar = i-(ndes+nmed); //Detecta el instante de parada 01047 velV = false; 01048 decel = false; 01049 mov = false; 01050 fin = true; 01051 t_Par = t; 01052 //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); 01053 } 01054 } 01055 } 01056 } 01057 01058 //////////////////////////////////////////////////////////////////////////// 01059 /// CALCULO DE POSICION Y VELOCIDAD INTEGRANDO LA ACELERACION Z FILTRADA /// 01060 //////////////////////////////////////////////////////////////////////////// 01061 01062 int i2 = i-(ndes+nmed); //Nuevo idice para calcular posicion y velocidad 01063 01064 01065 01066 //if (i2 == 0) {t_int = t;} //Ojo con los primeros valores 01067 01068 //actualizo valores t_3, aZ_3 01069 t_3[0] = t_3[1]; 01070 t_3[1] = t_3[2]; 01071 01072 t_3[2] = t; 01073 01074 aZ_3[0] = aZ_3[1]; 01075 aZ_3[1] = aZ_3[2]; 01076 aZ_3[2] = aXYZ[2]-aZm0; 01077 01078 if(i2 >= 0) 01079 { 01080 vZ_3[0] = vZ_3[1]; 01081 vZ_3[1] = vZ_3[2]; 01082 01083 xZ_3[0] = xZ_3[1]; 01084 xZ_3[1] = xZ_3[2]; 01085 01086 xa = t_3[2]; 01087 xb = t_3[1]; 01088 xc = t_3[0]; 01089 01090 h = (xa-xb)/2.0; 01091 01092 fa = aZ_3[2]; 01093 fb = aZ_3[1]; 01094 fc = aZ_3[0]; 01095 01096 //if(fabs(xa-xb)>1.0e-9) {fab = (fa-fb)/(xa-xb);} else {fab = 0.0;} 01097 //if(fabs(xb-xc)>1.0e-9) {fbc = (fb-fc)/(xb-xc);} else {fbc = 0.0;} 01098 //if(fabs(xa-xc)>1.0e-9) {fabc = (fab-fbc)/(xa-xc);} else {fabc = 0.0;} 01099 fab = (fa-fb)/(xa-xb); 01100 fbc = (fb-fc)/(xb-xc); 01101 fabc = (fab-fbc)/(xa-xc); 01102 01103 vZ_3[2]= vZ_3[1] + fa*2.0*h + fab*(-2.0*h*h) + fabc*(-4.0*h*h*h/6.0); 01104 01105 // Posicion 01106 01107 fa = vZ_3[2]; 01108 fb = vZ_3[1]; 01109 fc = vZ_3[0]; 01110 01111 //if(fabs(xa-xb)>1.0e-9) {fab = (fa-fb)/(xa-xb);} else {fab = 0.0;} 01112 //if(fabs(xb-xc)>1.0e-9) {fbc = (fb-fc)/(xb-xc);} else {fbc = 0.0;} 01113 //if(fabs(xa-xc)>1.0e-9) {fabc = (fab-fbc)/(xa-xc);} else {fabc = 0.0;} 01114 fab = (fa-fb)/(xa-xb); 01115 fbc = (fb-fc)/(xb-xc); 01116 fabc = (fab-fbc)/(xa-xc); 01117 01118 xZ_3[2] = xZ_3[1] + fa*2.0*h + fab*(-2.0*h*h) + fabc*(-4.0*h*h*h/6.0); 01119 01120 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 01121 } 01122 01123 //CALCULO DE LOS DESPLAZAMIENTOS EN INSTANTES DE ARRANQUE Y PARADA 01124 01125 if(i-(ndes+nmed) == nArr) {d_arr = xZ_3[2];} 01126 if(i-(ndes+nmed) == nPar) {d_par = xZ_3[2];} 01127 01128 //CALCULO VELOCIDAD MEDIA EN INSTANTES FINALES 01129 //PARA IMPLEMENTAR EL TRUCO DE DANI 01130 01131 if((ContadorDatos-i) <= nmedV) 01132 { 01133 vZmFin += vZ_3[2]/(float)nmedV; 01134 tFin = t_3[2]; 01135 } 01136 01137 //Calculo de las velociades obtenidas por integración (que deben ser 0 en realidad) en los instantes de arranque y parada 01138 01139 if(nArr == (i-(ndes+nmed))) {vArr = vZ_3[2];} 01140 if(nPar == (i-(ndes+nmed))) {vPar = vZ_3[2];} 01141 01142 01143 }//Fin loop principal 01144 01145 01146 01147 fclose(fp); 01148 01149 fclose(fp3); 01150 01151 01152 01153 } 01154 01155 void DatosConfort(unsigned int ContadorEnsayos, unsigned int ContadorDatos, int PasoTiempo) 01156 { 01157 float t; 01158 float dZ, vZ, aZ; 01159 01160 const float tolV = 0.01; //Tolerancia, en tanto por uno de la velocidad nominal, para recalcular el tiempo final del viaje 01161 01162 unsigned int RecalculaNPar = 0; //Se pone a 1 para recalcular el instante de parada 01163 unsigned int CalculaBo1 = 0; //Se pone a 1 para calcular el limite Bo1 01164 unsigned int CalculaBo2 = 0; //Se pone a 1 para calcular el limite Bo2 01165 01166 Bo0 = 0; //Se establece el Bo0 al principio, tras los puntos descartados y los de calculo de g 01167 01168 Bo3 = ContadorDatos-(ndes+nmed)-1; //Se establece el Bo3 al final, contando los puntos descartados y los de calculo de g 01169 if(Bo3>4000000000){Bo3 = 0;} 01170 01171 Boacel = (unsigned int)(nV0 + nVfin)*0.5; //Se establece inicialmente (luego se vera si tenemos que cambiarlo) como la media entre nV0 y nV 01172 01173 //float amax = 0.0;//VALOR POR PANTALLA 01174 //float dmax = 0.0;//VALOR POR PANTALLA 01175 //float Vmax = 0.0;//VALOR POR PANTALLA 01176 01177 //Relativos al calculo del jerk 01178 const float running_time = 0.5; 01179 int Ai = (int)(0.5*running_time*1.0e6)/PasoTiempo; 01180 float aZ_05s[2*Ai+1]; //Aceleraciones dentro del periodo de 0.5 segundos para hacer regresion lineal 01181 float jerk_025s[Ai+1]; //Jerks dentro del periodo de 0.25 01182 for (unsigned int i = 0; i<(2*Ai+1); i++) { aZ_05s[i] = 0.0; } 01183 for (unsigned int i = 0; i<(Ai+1); i++) { jerk_025s[i] = 0.0; } 01184 01185 ////////////////////////////////////////////////////////////////////////////// 01186 // Datos para nueva integración de la velocidad para obtener desplazamiento // 01187 ////////////////////////////////////////////////////////////////////////////// 01188 01189 float t_3[3], vZ_3[3], xZ_3[3]; //Datos guardados para hacer integracion numerica de aceleraciones 01190 float xa, xb, xc, h, fa, fb, fc, fab, fbc, fabc; 01191 01192 01193 for (unsigned int i = 0; i<3; i++) 01194 { 01195 t_3[i] = 0.0; vZ_3[i] = 0.0; xZ_3[i] = 0.0; 01196 } 01197 //nuevo Juan, soluciona error en nuevo calculo de dZ 01198 t_3[2] = (ndes + nmed - 1)*PasoTiempo/1000000.0; 01199 t_3[1] = (ndes + nmed - 2)*PasoTiempo/1000000.0; 01200 01201 //Archivos de lectura y escritura 01202 01203 FILE * fp; 01204 FILE * fp2; 01205 01206 char NoViajel[2]; 01207 char dispVelFile[50]; char velMod[50]; 01208 01209 strcpy(dispVelFile,"/local/dva0_"); strcpy(velMod,"/local/dvaj_"); 01210 01211 sprintf (NoViajel, "%d", ContadorEnsayos); 01212 strcat(dispVelFile,NoViajel); strcat(velMod,NoViajel); 01213 strcat(dispVelFile,".txt"); strcat(velMod,".txt"); 01214 01215 fp = fopen(dispVelFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", dispVelFile); } 01216 fp2 = fopen(velMod,"w"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", velMod); } 01217 01218 01219 //Lee velocidad para establecer periodos y recalcular nPar. Tambnien se calculan Bo1 y Bo2 01220 01221 int nDatos = (ContadorDatos-(ndes+nmed)); 01222 if (nDatos < 0) {nDatos = 0;} 01223 01224 float t_in; 01225 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 01226 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 01227 01228 for(int i = 0; i < nDatos; i++) 01229 { 01230 fscanf(fp,"%f %f %f %f", &t, &dZ, &vZ, &aZ);//aZ filtered, aX y aY raw. 01231 01232 if(i == 0) { t_in = t;} 01233 01234 //vZ = vZ - vZmFin*(t/tFin); //CAMBIO JL (11/10/2013) CON ESTA FORMA LA VELOCIDAD NO PARTE DE 0 01235 //vZ = vZ - vZmFin*((t-t_in)/(tFin-t_in)); 01236 01237 //Truco de Dani mas sofisticado 01238 if(i < nArr) { vZ = vZ - vArr*((t-t_in)/(t_Arr-t_in));} 01239 if(i >= nArr) { vZ = vZ - (a_LV*t + b_LV);} 01240 01241 01242 // NUEVA INTEGRACIÓN DE LA VELOCIDAD PARA CALCULAR DESPLAZAMIENTO 01243 01244 t_3[0] = t_3[1]; 01245 t_3[1] = t_3[2]; 01246 t_3[2] = t; 01247 01248 vZ_3[0] = vZ_3[1]; 01249 vZ_3[1] = vZ_3[2]; 01250 vZ_3[2] = vZ; 01251 01252 xZ_3[0] = xZ_3[1]; 01253 xZ_3[1] = xZ_3[2]; 01254 01255 xa = t_3[2]; 01256 xb = t_3[1]; 01257 xc = t_3[0]; 01258 01259 h = (xa-xb)/2.0; 01260 01261 fa = vZ_3[2]; 01262 fb = vZ_3[1]; 01263 fc = vZ_3[0]; 01264 01265 //if(fabs(xa-xb)>1.0e-9) {fab = (fa-fb)/(xa-xb);} else {fab = 0.0;} 01266 //if(fabs(xb-xc)>1.0e-9) {fbc = (fb-fc)/(xb-xc);} else {fbc = 0.0;} 01267 //if(fabs(xa-xc)>1.0e-9) {fabc = (fab-fbc)/(xa-xc);} else {fabc = 0.0;} 01268 fab = (fa-fb)/(xa-xb); 01269 fbc = (fb-fc)/(xb-xc); 01270 fabc = (fab-fbc)/(xa-xc); 01271 01272 xZ_3[2] = xZ_3[1] + fa*2.0*h + fab*(-2.0*h*h) + fabc*(-4.0*h*h*h/6.0); 01273 01274 dZ = xZ_3[2]; 01275 01276 if(i == nArr) {d_arr = dZ;} 01277 if(i == nPar) {d_par = dZ;} 01278 01279 // HASTA AQUÍ 01280 01281 01282 if( (i>=nV0)&&(i<nVfin) ) 01283 { 01284 vNom += fabs(vZ)/(float)(nVfin-nV0); 01285 } 01286 01287 if( (i == nPar) && (fabs(vZ) > tolV*fabs(vNom)) ) 01288 { 01289 RecalculaNPar = 1; 01290 } 01291 01292 if( (RecalculaNPar == 1) && (fabs(vZ) < tolV*fabs(vNom)) ) 01293 { 01294 nPar = i; 01295 t_Par = t; 01296 RecalculaNPar = 0; 01297 } 01298 01299 // Calculo de Bo1 01300 01301 if( i == nArr) { CalculaBo1 = 1; } 01302 01303 if ((fabs(fabs(dZ)-fabs(d_arr))>0.5) && (CalculaBo1 == 1)) 01304 { 01305 Bo1 = i; 01306 CalculaBo1 = 0; 01307 } 01308 //OJO: INTENTAR CORREGIR Bo1 CON NUEVO PUNTO DE PARADA arranque 01309 01310 // Calculo de Bo2 01311 01312 //OJO: INTENTAR CORREGIR Bo2 CON NUEVO PUNTO DE PARADA nPar 01313 01314 if( i == nV0) { CalculaBo2 = 1; } 01315 01316 if ((fabs(fabs(dZ)-fabs(d_par))<0.5) && (CalculaBo2 == 1) && (i>Bo1)) 01317 { 01318 Bo2 = i; 01319 CalculaBo2 = 0; 01320 } 01321 01322 //Calculo maxima aceleracion y deceleracion. Calcula Vmax1 y Vmax2. n05Vmaxi y n95Vmaxi en la siguiente pasada. 01323 01324 if ( (i>=Bo0) && (i<Boacel)) 01325 { 01326 if(fabs(aZ)>amax) { amax = fabs(aZ);} 01327 if(fabs(vZ)>Vmax1) { Vmax1 = fabs(vZ);} 01328 } 01329 01330 if ( (i>=Boacel) && (i<=Bo3)) 01331 { 01332 if(fabs(aZ)>dmax) { dmax = fabs(aZ);} 01333 if(fabs(vZ)>Vmax2) { Vmax2 = fabs(vZ);} 01334 } 01335 01336 //Calculo de jerk por regresion 01337 01338 for(unsigned int j = 0; j<(2*Ai+1); j++) { aZ_05s[j] = aZ_05s[j+1]; } 01339 aZ_05s[2*Ai] = aZ; 01340 for(unsigned int j = 0; j<(Ai+1); j++) { jerk_025s[j] = jerk_025s[j+1]; } 01341 jerk_025s[Ai] = RegresionJerk(aZ_05s, 2*Ai+1); 01342 01343 //Ojo, el jerk esta desfasado 0.25 segundos. Claculo su m�xdimo en la siguiente pasada. 01344 01345 //calculo Vmax a partir de Vmax1 y Vmax2. 01346 if (Vmax1<Vmax2) 01347 { 01348 Vmax = Vmax2; 01349 }else 01350 { 01351 Vmax = Vmax1; 01352 } 01353 01354 if(i== nPar) {d_par = dZ;} 01355 01356 01357 fprintf(fp2,"%f %f %f %f %f \n", t, dZ, vZ, aZ, jerk_025s[0]); 01358 } 01359 01360 01361 01362 01363 fclose(fp); 01364 fclose(fp2); 01365 01366 01367 01368 //ELIMINA ARCHIVO dva0 01369 int rmDVA0 = remove(dispVelFile); if(rmDVA0 != 0) { display.Clear();display.SetXY(0,0); display.printf("Error removing %s file\n", dispVelFile); } 01370 01371 } 01372 01373 void calcula_n05n95_Boveloc_jerkmax(unsigned int ContadorEnsayos, unsigned int ContadorDatos, int PasoTiempo) 01374 { 01375 01376 //float jerkmax = 0.0;//POR PANTALLA 01377 float t; 01378 float dZ, vZ, aZ, jerk; 01379 int CalculaBo2(0); 01380 01381 //5% and 95% of Vmax1 y Vmax2 01382 unsigned int c1(0);unsigned int c2(0);unsigned int c3(0);unsigned int c4(0); 01383 01384 01385 //Velocidad A95 01386 const float A95max_veloc_acel = 0.95*Vmax1; 01387 const float A95max_veloc_decel = 0.95*Vmax2; 01388 unsigned int contA95 = Bo1; 01389 unsigned int contA95_2 = Boacel + 1; 01390 bool Bo_veloc_acel_done = false; 01391 bool Bo_veloc_decel_done = false; 01392 float t2(0.0); 01393 float t3(0.0); 01394 01395 //Archivos de lectura y escritura 01396 01397 FILE * fp; 01398 01399 char NoViajel[2]; 01400 char velMod[50]; 01401 01402 strcpy(velMod,"/local/dvaj_"); 01403 01404 sprintf (NoViajel, "%d", ContadorEnsayos); 01405 strcat(velMod,NoViajel); 01406 strcat(velMod,".txt"); 01407 01408 fp = fopen(velMod,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", velMod); } 01409 01410 int nDatos = (ContadorDatos-(ndes+nmed)); 01411 if (nDatos < 0) {nDatos = 0;} 01412 01413 //loop principal 01414 for(unsigned int i = 0; i<nDatos; i++) 01415 { 01416 //read data row 01417 fscanf(fp,"%f %f %f %f %f", &t, &dZ, &vZ, &aZ, &jerk); 01418 01419 if( i == nV0) { CalculaBo2 = 1; } 01420 01421 if ( (fabs(fabs(dZ)-fabs(d_par))<0.5) && (i>Bo1) && (CalculaBo2 == 1) ) 01422 { 01423 Bo2 = i; 01424 CalculaBo2 = 0; 01425 } 01426 01427 //jerkmax 01428 if ( fabs(jerk) >= jerkmax ) 01429 { 01430 jerkmax = fabs(jerk); 01431 } 01432 01433 //5% and 95% of Vmax1 01434 if ( (i >= Bo0) && (i <= Boacel) ) 01435 { 01436 if( ( fabs(vZ) > 0.05*Vmax1 ) && (c1 == 0) ) 01437 { 01438 n05Vmax1 = i; 01439 c1 = 1; 01440 } 01441 if( ( fabs(vZ) > 0.95*Vmax1 ) && (c2 == 0) ) 01442 { 01443 n95Vmax1 = i; 01444 c2 = 1; 01445 } 01446 } 01447 01448 //5% and 95% of Vmax2 01449 if ( (i >= (Boacel + 1)) && (i < Bo3) ) 01450 { 01451 if( ( fabs(vZ) < 0.95*Vmax2 ) && (c3 == 0) ) 01452 { 01453 n95Vmax2 = i; 01454 c3 = 1; 01455 } 01456 if( ( fabs(vZ) < 0.05*Vmax2 ) && (c4 == 0) ) 01457 { 01458 n05Vmax2 = i; 01459 c4 = 1; 01460 } 01461 } 01462 01463 //Bo_veloc_acel y Bo_veloc_decel 01464 if( ( (i > Bo1) && (i<=Boacel) ) && ( Bo_veloc_acel_done == false ) ) 01465 { 01466 if ( fabs(vZ) >= A95max_veloc_acel ) 01467 { 01468 Bo_veloc_acel = contA95; 01469 t2 = t + 1.0; 01470 Bo_veloc_acel_done = true; 01471 } 01472 contA95++; 01473 } 01474 if( ( (i > (Boacel + 1)) && (i<=Bo3)) && (Bo_veloc_decel_done == false) ) 01475 { 01476 if ( fabs(vZ) >= A95max_veloc_decel ) 01477 { 01478 Bo_veloc_decel = contA95_2; 01479 t3 = t - 1.0; 01480 Bo_veloc_decel_done = true; 01481 } 01482 contA95_2++; 01483 } 01484 01485 01486 }//loop principal 01487 01488 //calculos after loop 01489 //t2 y t3 01490 01491 for (unsigned int j = 0 ; j<=100 ; j++) 01492 { 01493 if(t3<t2) 01494 { 01495 t2 = t2 - ((float)(1))/10.0; 01496 t3 = t3 + ((float)(1))/10.0; 01497 }else 01498 { 01499 Bo_veloc_acel = (unsigned int)( Bo_veloc_acel + (1.0/((float)(PasoTiempo/1000000.0))) - ((float)(j))/(10.0*((float)(PasoTiempo/1000000.0))) ); 01500 Bo_veloc_decel = (unsigned int)( Bo_veloc_decel - (1.0/((float)(PasoTiempo/1000000.0))) + ((float)(j))/(10.0*((float)(PasoTiempo/1000000.0))) ); 01501 break; 01502 } 01503 } 01504 01505 01506 01507 01508 //seguridad 01509 //seguridad por si nVmax1 01510 if ( c1 == 0 ) 01511 { 01512 n05Vmax1 = (unsigned int)(Bo0 + (Bo1-Bo0)/2.0); 01513 } 01514 if ( c2 == 0 ) 01515 { 01516 n95Vmax1 = Boacel; 01517 } 01518 //seguridad por si nVmax2 01519 if ( c3 == 0 ) 01520 { 01521 n95Vmax2 = Boacel; 01522 } 01523 if ( c4 == 0 ) 01524 { 01525 n05Vmax2 = (unsigned int)(Bo2 + (Bo3-Bo2)/2.0); 01526 } 01527 //cierra ficheros 01528 fclose(fp); 01529 01530 01531 } 01532 01533 01534 01535 void calcula_A95_V_acel_decel(unsigned int ContadorEnsayos, unsigned int ContadorDatos, int PasoTiempo) 01536 01537 { 01538 float t; 01539 float dZ, vZ, aZ, jerk; 01540 01541 //VA95 01542 int length_maximoVA95(0); 01543 if( Bo_veloc_decel >= Bo_veloc_acel ) 01544 { 01545 length_maximoVA95 = (int)((Bo_veloc_decel - Bo_veloc_acel)*0.05) + 1; 01546 }else 01547 { 01548 length_maximoVA95 = 1; 01549 } 01550 //int length_maximoVA95 = (int)((Bo_veloc_decel - Bo_veloc_acel)*0.05) + 1; 01551 01552 int maxindex(0); 01553 vector<float> *maximoAV95 = new vector<float> (length_maximoVA95,0.0); 01554 //vector<float> maximoAV95 (length_maximoVA95,0.0); 01555 vector<float> *diferencia_vec = new vector<float>; 01556 vector<float>::iterator up; 01557 float diferencia; 01558 float maximo; 01559 int contadorA95(0); 01560 //aA95 01561 int length_maximo_aA95(0); 01562 if( n95Vmax1 >= n05Vmax1 ) 01563 { 01564 length_maximo_aA95 = (int)((n95Vmax1 - n05Vmax1)*0.05) + 1; 01565 }else 01566 { 01567 length_maximo_aA95 = 1; 01568 } 01569 //int length_maximo_aA95 = (int)((n95Vmax1 - n05Vmax1)*0.05) + 1; 01570 01571 int maxindex_aA95(0); 01572 vector<float> *maximo_aA95 = new vector<float> (length_maximo_aA95,0.0); 01573 //vector<float> maximo_aA95 (length_maximo_aA95,0.0); 01574 vector<float> *diferencia_aA95_vec = new vector<float>; 01575 vector<float>::iterator up_aA95; 01576 float diferencia_aA95; 01577 float maximoaA95; 01578 int contador_aA95(0); 01579 //dA95 01580 int length_maximo_dA95(0); 01581 if( n05Vmax2 >= n95Vmax2 ) 01582 { 01583 length_maximo_dA95 = (int)((n05Vmax2 - n95Vmax2)*0.05) + 1; 01584 }else 01585 { 01586 length_maximo_dA95 = 1; 01587 } 01588 //int length_maximo_dA95 = (int)((n05Vmax2 - n95Vmax2)*0.05) + 1; 01589 01590 int maxindex_dA95(0); 01591 vector<float> *maximo_dA95 = new vector<float> (length_maximo_dA95,0.0); 01592 //vector<float> maximo_dA95 (length_maximo_dA95,0.0); 01593 vector<float> *diferencia_dA95_vec = new vector<float>; 01594 vector<float>::iterator up_dA95; 01595 float diferencia_dA95; 01596 float maximodA95; 01597 int contador_dA95(0); 01598 01599 //Archivos de lectura y escritura 01600 01601 FILE * fp; 01602 01603 char NoViajel[2]; 01604 char data[50]; 01605 01606 strcpy(data,"/local/dvaj_"); 01607 sprintf (NoViajel, "%d", ContadorEnsayos); 01608 strcat(data,NoViajel); 01609 strcat(data,".txt"); 01610 01611 fp = fopen(data,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", data); } 01612 01613 int nDatos = (ContadorDatos-(ndes+nmed)); 01614 if (nDatos < 0) {nDatos = 0;} 01615 01616 01617 if ( (maximoAV95 == NULL) || ( maximo_aA95 == NULL) || ( maximo_dA95 == NULL) ) 01618 { 01619 display.Clear();display.SetXY(0,0); display.printf("RAM overflow\n"); 01620 }else 01621 { 01622 //loop principal, pasada 1 01623 for(unsigned int i = 0; i<(nDatos); i++) 01624 { 01625 //read data row 01626 fscanf(fp,"%f %f %f %f %f", &t, &dZ, &vZ, &aZ, &jerk); 01627 01628 //VA95 01629 if( (i <= Bo_veloc_decel) && ( i >= Bo_veloc_acel) ) 01630 { 01631 01632 //pasada 1 01633 for (unsigned int j = 0; j<length_maximoVA95; j++) 01634 { 01635 01636 diferencia = fabs(vZ) - maximoAV95->at(j); 01637 if ( diferencia < 0 ) 01638 { 01639 diferencia_vec->push_back(0.0); 01640 contadorA95++; 01641 }else 01642 { 01643 diferencia_vec->push_back(diferencia); 01644 } 01645 01646 } 01647 if ( contadorA95 == length_maximoVA95 ){} 01648 else 01649 { 01650 01651 maximo = *max_element(diferencia_vec->begin(),diferencia_vec->end()); 01652 up = find(diferencia_vec->begin(), diferencia_vec->end(), maximo); 01653 maxindex = int(up - diferencia_vec->begin()); 01654 maximoAV95->erase(maximoAV95->begin() + maxindex ); 01655 maximoAV95->insert(maximoAV95->begin() + maxindex ,fabs(vZ)); 01656 } 01657 contadorA95 = 0; 01658 diferencia_vec->clear(); 01659 } 01660 01661 //aA95 01662 if( (i <= n95Vmax1) && ( i >= n05Vmax1) ) 01663 { 01664 01665 //pasada 1 01666 for (unsigned int j = 0; j<length_maximo_aA95; j++) 01667 { 01668 01669 diferencia_aA95 = fabs(aZ) - maximo_aA95->at(j); 01670 if ( diferencia_aA95 < 0 ) 01671 { 01672 diferencia_aA95_vec->push_back(0.0); 01673 contador_aA95++; 01674 }else 01675 { 01676 diferencia_aA95_vec->push_back(diferencia_aA95); 01677 } 01678 01679 } 01680 if ( contador_aA95 == length_maximo_aA95 ){} 01681 else 01682 { 01683 01684 maximoaA95 = *max_element(diferencia_aA95_vec->begin(),diferencia_aA95_vec->end()); 01685 up_aA95 = find(diferencia_aA95_vec->begin(), diferencia_aA95_vec->end(), maximoaA95); 01686 maxindex_aA95 = int(up_aA95 - diferencia_aA95_vec->begin()); 01687 maximo_aA95->erase(maximo_aA95->begin() + maxindex_aA95 ); 01688 maximo_aA95->insert(maximo_aA95->begin() + maxindex_aA95 ,fabs(aZ)); 01689 } 01690 contador_aA95 = 0; 01691 diferencia_aA95_vec->clear(); 01692 } 01693 01694 //dA95 01695 if( (i <= n05Vmax2) && ( i >= n95Vmax2) ) 01696 { 01697 01698 //pasada 1 01699 for (unsigned int j = 0; j<length_maximo_dA95; j++) 01700 { 01701 01702 diferencia_dA95 = fabs(aZ) - maximo_dA95->at(j); 01703 if ( diferencia_dA95 < 0 ) 01704 { 01705 diferencia_dA95_vec->push_back(0.0); 01706 contador_dA95++; 01707 }else 01708 { 01709 diferencia_dA95_vec->push_back(diferencia_dA95); 01710 } 01711 01712 } 01713 if ( contador_dA95 == length_maximo_dA95 ){} 01714 else 01715 { 01716 01717 maximodA95 = *max_element(diferencia_dA95_vec->begin(),diferencia_dA95_vec->end()); 01718 up_dA95 = find(diferencia_dA95_vec->begin(), diferencia_dA95_vec->end(), maximodA95); 01719 maxindex_dA95 = int(up_dA95 - diferencia_dA95_vec->begin()); 01720 maximo_dA95->erase(maximo_dA95->begin() + maxindex_dA95 ); 01721 maximo_dA95->insert(maximo_dA95->begin() + maxindex_dA95 ,fabs(aZ)); 01722 } 01723 contador_dA95 = 0; 01724 diferencia_dA95_vec->clear(); 01725 } 01726 01727 }//Fin loop principal, pasada 1. 01728 01729 sort(maximoAV95->begin(),maximoAV95->end());//ascending 01730 reverse(maximoAV95->begin(),maximoAV95->end());//descending 01731 sort(maximo_aA95->begin(),maximo_aA95->end());//ascending 01732 reverse(maximo_aA95->begin(),maximo_aA95->end());//descending 01733 sort(maximo_dA95->begin(),maximo_dA95->end());//ascending 01734 reverse(maximo_dA95->begin(),maximo_dA95->end());//descending 01735 01736 01737 //cierra ficheros y variables 01738 fclose(fp); 01739 01740 //pasada2 01741 fp = fopen(data,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", data); } 01742 01743 01744 //loop principal, pasada 2 01745 double resta(1000.0); 01746 double resta_aA95(1000.0); 01747 double resta_dA95(1000.0); 01748 for(unsigned int i = 0; i<(nDatos); i++) 01749 { 01750 //read data row 01751 fscanf(fp,"%f %f %f %f %f", &t, &dZ, &vZ, &aZ, &jerk); 01752 01753 //VA95 01754 01755 if( (i <= Bo_veloc_decel) && ( i >= Bo_veloc_acel) ) 01756 { 01757 if( ((maximoAV95->at(maximoAV95->size() - 1) - fabs(vZ)) <= resta) && (fabs(vZ) < maximoAV95->at(maximoAV95->size() - 1)) ) 01758 { 01759 VA95 = fabs(vZ); 01760 resta = maximoAV95->at(maximoAV95->size() - 1) - fabs(vZ); 01761 } 01762 } 01763 //aA95 01764 01765 if( (i <= n95Vmax1) && ( i >= n05Vmax1) ) 01766 { 01767 if( ((maximo_aA95->at(maximo_aA95->size() - 1) - fabs(aZ)) <= resta_aA95) && (fabs(aZ) < maximo_aA95->at(maximo_aA95->size() - 1)) ) 01768 { 01769 aA95 = fabs(aZ); 01770 resta_aA95 = maximo_aA95->at(maximo_aA95->size() - 1) - fabs(aZ); 01771 } 01772 } 01773 //dA95 01774 01775 01776 01777 if( (i <= n05Vmax2) && ( i >= n95Vmax2) ) 01778 { 01779 if( ((maximo_dA95->at(maximo_dA95->size() - 1) - fabs(aZ)) <= resta_dA95) && (fabs(aZ) < maximo_dA95->at(maximo_dA95->size() - 1)) ) 01780 { 01781 dA95 = fabs(aZ); 01782 resta_dA95 = maximo_dA95->at(maximo_dA95->size() - 1) - fabs(aZ); 01783 } 01784 } 01785 01786 01787 01788 } 01789 //clear all vectors 01790 maximoAV95->clear(); 01791 delete maximoAV95; 01792 diferencia_vec->clear(); 01793 delete diferencia_vec; 01794 maximo_aA95->clear(); 01795 delete maximo_aA95; 01796 diferencia_aA95_vec->clear(); 01797 delete diferencia_aA95_vec; 01798 maximo_dA95->clear(); 01799 delete maximo_dA95; 01800 diferencia_dA95_vec->clear(); 01801 delete diferencia_dA95_vec; 01802 01803 }//fin else 01804 01805 fclose(fp); 01806 01807 } 01808 01809 void vibraReadFilter(unsigned int ContadorEnsayos, unsigned int ContadorDatos) 01810 { 01811 01812 01813 float t; 01814 float aXYZ[3]; //vector aceleracion en el paso i [aX,aY,aZ] 01815 float Zvibweighted[3]={0.0,0.0,0.0}; 01816 float Xvibweighted[3]={0.0,0.0,0.0}; 01817 float Yvibweighted[3]={0.0,0.0,0.0}; 01818 float aZ[3], aX[3], aY[3]; 01819 01820 FILE * fp; 01821 FILE * fp3; 01822 01823 char NoViajel[2]; 01824 char resultsFile[50]; 01825 char resultsFileW[50]; 01826 01827 //strcpy(resultsFile,"/local/IV_"); 01828 strcpy(resultsFile,"/local/raw_V_"); 01829 01830 strcpy(resultsFileW,"/local/W_IV_"); 01831 01832 sprintf (NoViajel, "%d", ContadorEnsayos); 01833 strcat(resultsFile,NoViajel); strcat(resultsFileW,NoViajel);//fichero con todos los datos de la función anterior IV_ 01834 strcat(resultsFile,".txt"); strcat(resultsFileW,".txt"); 01835 01836 //fichero con todos los datos de la función anterior IV_ 01837 fp = fopen(resultsFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", resultsFile); } 01838 //fichero donde guardaré 01839 fp3 = fopen(resultsFileW,"w"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", resultsFileW); } 01840 01841 //varias pasadas para isofiltrar ax ay y az. isofilter son 4 filtros, pues 4 pasadas 01842 01843 //Hemos recordado 01844 01845 //loop principal. Pasada 1. Hl (Low Pass 100Hz Orden 2) 01846 for(unsigned int i = 0; i<(ContadorDatos); i++) 01847 { 01848 fscanf(fp,"%f %f %f %f", &t, &aXYZ[0], &aXYZ[1], &aXYZ[2]);//lee tiempo y 3 aceleraciones X, Y, Z 01849 //restamos gravedad a aZ 01850 aXYZ[2] = aXYZ[2] - aZm0; 01851 aXYZ[0] = aXYZ[0] - aXm0; 01852 aXYZ[1] = aXYZ[1] - aYm0; 01853 //isofilter 01854 01855 int i2 = i-(ndes+nmed); //Nuevo idice para calcular posicion y velocidad 01856 01857 01858 if ( i==0 ) 01859 { 01860 Zvibweighted[0] = aXYZ[2]; 01861 Xvibweighted[0] = aXYZ[0]; 01862 Yvibweighted[0] = aXYZ[1]; 01863 aZ[0] =aXYZ[2]; 01864 aX[0] =aXYZ[0]; 01865 aY[0] =aXYZ[1]; 01866 01867 01868 // fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[0], Yvibweighted[0], Zvibweighted[0]); 01869 01870 }else if ( i==1 ) 01871 { 01872 Zvibweighted[1] = aXYZ[2]; 01873 Xvibweighted[1] = aXYZ[0]; 01874 Yvibweighted[1] = aXYZ[1]; 01875 aZ[1] =aXYZ[2]; 01876 aX[1] =aXYZ[0]; 01877 aY[1] =aXYZ[1]; 01878 01879 // fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[1], Yvibweighted[1], Zvibweighted[1]); 01880 01881 }else 01882 { 01883 01884 aZ[2] = aXYZ[2]; 01885 aX[2] = aXYZ[0]; 01886 aY[2] = aXYZ[1]; 01887 01888 Zvibweighted[2] = isoFilter_Wk_z_Hl(Zvibweighted[0],Zvibweighted[1],aZ, PasoTiempo); 01889 Xvibweighted[2] = isoFilter_Wd_xy_Hl(Xvibweighted[0],Xvibweighted[1],aX, PasoTiempo); 01890 Yvibweighted[2] = isoFilter_Wd_xy_Hl(Yvibweighted[0],Yvibweighted[1],aY, PasoTiempo); 01891 01892 if(i2 >= 0) 01893 { 01894 fprintf(fp3,"%f %f %f %f \n", (t), Xvibweighted[2], Yvibweighted[2], Zvibweighted[2]); 01895 } 01896 01897 //updates 01898 aZ[0] = aZ[1]; 01899 aZ[1] = aZ[2]; 01900 aX[0] = aX[1]; 01901 aX[1] = aX[2]; 01902 aY[0] = aY[1]; 01903 aY[1] = aY[2]; 01904 Zvibweighted[0] = Zvibweighted[1]; 01905 Zvibweighted[1] = Zvibweighted[2]; 01906 Xvibweighted[0] = Xvibweighted[1]; 01907 Xvibweighted[1] = Xvibweighted[2]; 01908 Yvibweighted[0] = Yvibweighted[1]; 01909 Yvibweighted[1] = Yvibweighted[2]; 01910 //azfilter1Hz 01911 01912 } 01913 01914 }//Fin Hl 01915 fclose(fp); 01916 fclose(fp3); 01917 01918 01919 01920 01921 ///////////////////////////////////////// 01922 //Segund pasada Hh. (High Pass 0.4Hz, Orden 2) 01923 strcpy(resultsFile,"/local/W_IV_"); 01924 strcpy(resultsFileW,"/local/W2_IV_"); 01925 01926 sprintf (NoViajel, "%d", ContadorEnsayos); 01927 strcat(resultsFile,NoViajel); 01928 strcat(resultsFileW,NoViajel); 01929 strcat(resultsFile,".txt"); 01930 strcat(resultsFileW,".txt"); 01931 01932 //fichero con todos los datos de la función anterior IV_ 01933 fp = fopen(resultsFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", resultsFile); } 01934 //fichero donde guardaré 01935 fp3 = fopen(resultsFileW,"w"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", resultsFileW); } 01936 01937 01938 01939 //loop principal. Pasada 2. Hh (High Pass 0.4Hz Orden 2) 01940 for(unsigned int i = 0; i<(ContadorDatos-(ndes+nmed)); i++) 01941 { 01942 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. 01943 01944 //isofilter 01945 01946 if ( i==0 ) 01947 { 01948 Zvibweighted[0] = aXYZ[2]; 01949 Xvibweighted[0] = aXYZ[0]; 01950 Yvibweighted[0] = aXYZ[1]; 01951 aZ[0] =aXYZ[2]; 01952 aX[0] =aXYZ[0]; 01953 aY[0] =aXYZ[1]; 01954 fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[0], Yvibweighted[0], Zvibweighted[0]); 01955 01956 }else if ( i==1 ) 01957 { 01958 Zvibweighted[1] = aXYZ[2]; 01959 Xvibweighted[1] = aXYZ[0]; 01960 Yvibweighted[1] = aXYZ[1]; 01961 aZ[1] =aXYZ[2]; 01962 aX[1] =aXYZ[0]; 01963 aY[1] =aXYZ[1]; 01964 fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[1], Yvibweighted[1], Zvibweighted[1]); 01965 01966 }else 01967 { 01968 01969 aZ[2] = aXYZ[2]; 01970 aX[2] = aXYZ[0]; 01971 aY[2] = aXYZ[1]; 01972 01973 // Zvibweighted[2] = aXYZ[2];//isoFilter_Wk_z_Hh(Zvibweighted[0],Zvibweighted[1],aZ); 01974 // Xvibweighted[2] = aXYZ[0];//isoFilter_Wd_xy_Hh(Xvibweighted[0],Xvibweighted[1],aX); //OJO: el filtro High Pass no funciona bien 01975 // Yvibweighted[2] = aXYZ[1];//isoFilter_Wd_xy_Hh(Yvibweighted[0],Yvibweighted[1],aY); 01976 Zvibweighted[2] = isoFilter_Wk_z_Hh(Zvibweighted[0],Zvibweighted[1],aZ, PasoTiempo); 01977 Xvibweighted[2] = isoFilter_Wd_xy_Hh(Xvibweighted[0],Xvibweighted[1],aX, PasoTiempo); 01978 Yvibweighted[2] = isoFilter_Wd_xy_Hh(Yvibweighted[0],Yvibweighted[1],aY, PasoTiempo); 01979 fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[2], Yvibweighted[2], Zvibweighted[2]); 01980 //updates 01981 aZ[0] = aZ[1]; 01982 aZ[1] = aZ[2]; 01983 aX[0] = aX[1]; 01984 aX[1] = aX[2]; 01985 aY[0] = aY[1]; 01986 aY[1] = aY[2]; 01987 Zvibweighted[0] = Zvibweighted[1]; 01988 Zvibweighted[1] = Zvibweighted[2]; 01989 Xvibweighted[0] = Xvibweighted[1]; 01990 Xvibweighted[1] = Xvibweighted[2]; 01991 Yvibweighted[0] = Yvibweighted[1]; 01992 Yvibweighted[1] = Yvibweighted[2]; 01993 } 01994 01995 }//Fin Hh1 01996 fclose(fp); 01997 fclose(fp3); 01998 int rmDVA0 = remove(resultsFile); 01999 02000 02001 ///////////////////////////////////////// 02002 //Tercera pasada Ht. (Ht filtro transicion) 02003 strcpy(resultsFile,"/local/W2_IV_"); 02004 strcpy(resultsFileW,"/local/W4_IV_"); 02005 02006 sprintf (NoViajel, "%d", ContadorEnsayos); 02007 strcat(resultsFile,NoViajel); 02008 strcat(resultsFileW,NoViajel); 02009 strcat(resultsFile,".txt"); 02010 strcat(resultsFileW,".txt"); 02011 02012 //fichero con todos los datos de la función anterior IV_ 02013 fp = fopen(resultsFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", resultsFile); } 02014 //fichero donde guardaré 02015 fp3 = fopen(resultsFileW,"w"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", resultsFileW); } 02016 02017 02018 02019 02020 //loop principal. Pasada 3. Ht (Transicion) 02021 for(unsigned int i = 0; i<(ContadorDatos-(ndes+nmed)); i++) 02022 { 02023 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. 02024 02025 //isofilter 02026 02027 if ( i==0 ) 02028 { 02029 Zvibweighted[0] = aXYZ[2]; 02030 Xvibweighted[0] = aXYZ[0]; 02031 Yvibweighted[0] = aXYZ[1]; 02032 aZ[0] =aXYZ[2]; 02033 aX[0] =aXYZ[0]; 02034 aY[0] =aXYZ[1]; 02035 fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[0], Yvibweighted[0], Zvibweighted[0]); 02036 02037 }else if ( i==1 ) 02038 { 02039 Zvibweighted[1] = aXYZ[2]; 02040 Xvibweighted[1] = aXYZ[0]; 02041 Yvibweighted[1] = aXYZ[1]; 02042 aZ[1] =aXYZ[2]; 02043 aX[1] =aXYZ[0]; 02044 aY[1] =aXYZ[1]; 02045 fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[1], Yvibweighted[1], Zvibweighted[1]); 02046 02047 }else 02048 { 02049 02050 aZ[2] = aXYZ[2]; 02051 aX[2] = aXYZ[0]; 02052 aY[2] = aXYZ[1]; 02053 02054 Zvibweighted[2] = isoFilter_Wk_z_Ht(Zvibweighted[0],Zvibweighted[1],aZ, PasoTiempo); 02055 Xvibweighted[2] = isoFilter_Wd_xy_Ht(Xvibweighted[0],Xvibweighted[1],aX, PasoTiempo); 02056 Yvibweighted[2] = isoFilter_Wd_xy_Ht(Yvibweighted[0],Yvibweighted[1],aY, PasoTiempo); 02057 // Zvibweighted[2] = aXYZ[2]; 02058 // Xvibweighted[2] = aXYZ[0]; 02059 // Yvibweighted[2] = aXYZ[1]; 02060 fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[2], Yvibweighted[2], Zvibweighted[2]); 02061 //updates 02062 aZ[0] = aZ[1]; 02063 aZ[1] = aZ[2]; 02064 aX[0] = aX[1]; 02065 aX[1] = aX[2]; 02066 aY[0] = aY[1]; 02067 aY[1] = aY[2]; 02068 Zvibweighted[0] = Zvibweighted[1]; 02069 Zvibweighted[1] = Zvibweighted[2]; 02070 Xvibweighted[0] = Xvibweighted[1]; 02071 Xvibweighted[1] = Xvibweighted[2]; 02072 Yvibweighted[0] = Yvibweighted[1]; 02073 Yvibweighted[1] = Yvibweighted[2]; 02074 } 02075 02076 }//Fin Ht 02077 fclose(fp); 02078 fclose(fp3); 02079 rmDVA0 = remove(resultsFile); 02080 02081 ///////////////////////////////////////// 02082 //Cuarta pasada Hs. (Hs filtro subida de escala) 02083 strcpy(resultsFile,"/local/W4_IV_"); 02084 strcpy(resultsFileW,"/local/W5_IV_"); 02085 02086 sprintf (NoViajel, "%d", ContadorEnsayos); 02087 strcat(resultsFile,NoViajel); 02088 strcat(resultsFileW,NoViajel); 02089 strcat(resultsFile,".txt"); 02090 strcat(resultsFileW,".txt"); 02091 02092 //fichero con todos los datos de la función anterior IV_ 02093 fp = fopen(resultsFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", resultsFile); } 02094 //fichero donde guardaré 02095 fp3 = fopen(resultsFileW,"w"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", resultsFileW); } 02096 02097 //loop principal. Pasada 4. Hs (subida escala) 02098 02099 02100 02101 02102 for(unsigned int i = 0; i<(ContadorDatos-(ndes+nmed)); i++) 02103 { 02104 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. 02105 02106 //isofilter 02107 02108 if ( i==0 ) 02109 { 02110 Zvibweighted[0] = aXYZ[2]; 02111 Xvibweighted[0] = aXYZ[0]; 02112 Yvibweighted[0] = aXYZ[1]; 02113 aZ[0] =aXYZ[2]; 02114 aX[0] =aXYZ[0]; 02115 aY[0] =aXYZ[1]; 02116 02117 fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[0], Yvibweighted[0], Zvibweighted[0]); 02118 02119 }else if ( i==1 ) 02120 { 02121 Zvibweighted[1] = aXYZ[2]; 02122 Xvibweighted[1] = aXYZ[0]; 02123 Yvibweighted[1] = aXYZ[1]; 02124 aZ[1] =aXYZ[2]; 02125 aX[1] =aXYZ[0]; 02126 aY[1] =aXYZ[1]; 02127 02128 fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[1], Yvibweighted[1], Zvibweighted[1]); 02129 02130 }else 02131 { 02132 02133 aZ[2] = aXYZ[2]; 02134 aX[2] = aXYZ[0]; 02135 aY[2] = aXYZ[1]; 02136 02137 Zvibweighted[2] = isoFilter_Wk_z_Hs(Zvibweighted[0],Zvibweighted[1],aZ, PasoTiempo); 02138 Xvibweighted[2] = aX[2]; 02139 Yvibweighted[2] = aY[2]; 02140 // Zvibweighted[2] = aXYZ[2]; 02141 // Xvibweighted[2] = aX[2]; 02142 // Yvibweighted[2] = aY[2]; 02143 02144 fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[2], Yvibweighted[2], Zvibweighted[2]); 02145 //updates 02146 aZ[0] = aZ[1]; 02147 aZ[1] = aZ[2]; 02148 aX[0] = aX[1]; 02149 aX[1] = aX[2]; 02150 aY[0] = aY[1]; 02151 aY[1] = aY[2]; 02152 Zvibweighted[0] = Zvibweighted[1]; 02153 Zvibweighted[1] = Zvibweighted[2]; 02154 Xvibweighted[0] = Xvibweighted[1]; 02155 Xvibweighted[1] = Xvibweighted[2]; 02156 Yvibweighted[0] = Yvibweighted[1]; 02157 Yvibweighted[1] = Yvibweighted[2]; 02158 02159 } 02160 02161 }//Fin Ht 02162 fclose(fp); 02163 fclose(fp3); 02164 rmDVA0 = remove(resultsFile); 02165 } 02166 02167 02168 void calculaStep2(unsigned int ContadorEnsayos, unsigned int ContadorDatos) 02169 { 02170 //calcula step2 previo filtadro deaZ ya filtrada a 10Hz, a 1Hz esta vez. 02171 float t; 02172 float dZ,vZ,aZfilter10Hz,jerk,aZ[3]; 02173 float azfilter1Hz[3]; 02174 //Calculo de step2 02175 const float running_time = 1.0; 02176 02177 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 02178 02179 float azfilter1Hz_1s[2*Ai+1]; //Aceleraciones dentro del periodo de 0.5 segundos para hacer regresion lineal 02180 float step2_05s[Ai+1]; //Jerks dentro del periodo de 0.25 02181 for (unsigned int i = 0; i<(2*Ai+1); i++) { azfilter1Hz_1s[i] = 0.0; } 02182 02183 for (unsigned int i = 0; i<(Ai+1); i++) { step2_05s[i] = 0.0; } 02184 02185 //Archivos de lectura y escritura 02186 02187 FILE * fp; 02188 FILE * fp2; 02189 02190 char NoViajel[2]; 02191 char dispVelFile[50]; char velMod[50]; 02192 02193 strcpy(dispVelFile,"/local/dvaj_"); 02194 strcpy(velMod,"/local/dvaja_"); 02195 02196 sprintf (NoViajel, "%d", ContadorEnsayos); 02197 strcat(dispVelFile,NoViajel); strcat(velMod,NoViajel); 02198 strcat(dispVelFile,".txt"); strcat(velMod,".txt"); 02199 02200 fp = fopen(dispVelFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", dispVelFile); } 02201 fp2 = fopen(velMod,"w"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", velMod); } 02202 02203 //const float fs = (1000000.0/PasoTiempo);//frecuencia de muestreo debe valer 320 02204 02205 //Primera pasada. calcula azfilter1Hz 02206 02207 int nDatos = (ContadorDatos-(ndes+nmed)); 02208 if (nDatos < 0) {nDatos = 0;} 02209 02210 for(unsigned int i = 0; i<nDatos; i++) 02211 { 02212 //fscanf(fp,"%f %f %f %f %f %f", &t, &dZ, &vZ, &aZ, &aX, &aY);//aZ filtered, aX y aY raw. 02213 fscanf(fp,"%f %f %f %f %f", &t, &dZ, &vZ, &aZfilter10Hz, &jerk); 02214 // 02215 //isofilter 02216 if ( i==0 ) 02217 { 02218 //azfilter1Hz 02219 aZ[0] = aZfilter10Hz; 02220 azfilter1Hz[0] = aZfilter10Hz; 02221 fprintf(fp2,"%f %f %f %f %f %f \n", t, dZ, vZ, aZfilter10Hz, jerk, azfilter1Hz[0]); 02222 02223 }else if ( i==1 ) 02224 { 02225 //azfilter1Hz 02226 aZ[1] = aZfilter10Hz; 02227 azfilter1Hz[1] = aZfilter10Hz; 02228 02229 fprintf(fp2,"%f %f %f %f %f %f \n", t, dZ, vZ, aZfilter10Hz, jerk, azfilter1Hz[1]); 02230 02231 }else 02232 { 02233 //azfilter1Hz 02234 aZ[2] = aZfilter10Hz; 02235 if ( FrecAdq == 320 ) 02236 { 02237 azfilter1Hz[2] = toLowPass1HzO2_320(azfilter1Hz[0],azfilter1Hz[1],aZ); 02238 02239 }else if ( FrecAdq == 1000 ) 02240 { 02241 azfilter1Hz[2] = toLowPass1HzO2_1000(azfilter1Hz[0],azfilter1Hz[1],aZ); 02242 }else//seguridad 02243 { 02244 azfilter1Hz[2] = toLowPass1HzO2_320(azfilter1Hz[0],azfilter1Hz[1],aZ); 02245 } 02246 02247 fprintf(fp2,"%f %f %f %f %f %f \n", t, dZ, vZ, aZfilter10Hz, jerk, azfilter1Hz[2]); 02248 //updates 02249 aZ[0] = aZ[1]; 02250 aZ[1] = aZ[2]; 02251 //azfilter1Hz 02252 azfilter1Hz[0] = azfilter1Hz[1]; 02253 azfilter1Hz[1] = azfilter1Hz[2]; 02254 } 02255 02256 }//fin pasada 02257 fclose(fp); 02258 fclose(fp2); 02259 //int rmDVA0 = remove(dispVelFile);//elimina dvaj_ 02260 02261 //segunda pasada. Calcula step2 02262 strcpy(dispVelFile,"/local/dvaja_"); 02263 strcpy(velMod,"/local/dvaj2_"); 02264 02265 sprintf (NoViajel, "%d", ContadorEnsayos); 02266 strcat(dispVelFile,NoViajel); strcat(velMod,NoViajel); 02267 strcat(dispVelFile,".txt"); strcat(velMod,".txt"); 02268 02269 fp = fopen(dispVelFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", dispVelFile); } 02270 fp2 = fopen(velMod,"w"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", velMod); } 02271 02272 if (nDatos < 0) {nDatos = 0;} 02273 02274 for(unsigned int i = 0; i<nDatos; i++) 02275 { 02276 //fscanf(fp,"%f %f %f %f %f %f", &t, &dZ, &vZ, &aZ, &aX, &aY);//aZ filtered, aX y aY raw. 02277 fscanf(fp,"%f %f %f %f %f %f", &t, &dZ, &vZ, &aZfilter10Hz, &jerk, &azfilter1Hz[2]); 02278 //Calculo de step2 por regresion 02279 02280 for(unsigned int j = 0; j<(2*Ai+1); j++) { azfilter1Hz_1s[j] = azfilter1Hz_1s[j+1]; } 02281 azfilter1Hz_1s[2*Ai] = azfilter1Hz[2]; 02282 for(unsigned int j = 0; j<(Ai+1); j++) { step2_05s[j] = step2_05s[j+1]; } 02283 step2_05s[Ai] = RegresionJerk(azfilter1Hz_1s, 2*Ai+1); 02284 02285 fprintf(fp2,"%f %f %f %f %f %f \n", t, dZ, vZ, aZfilter10Hz, jerk, step2_05s[0]); 02286 } 02287 02288 fclose(fp); 02289 fclose(fp2); 02290 int rmDVA1 = remove(dispVelFile); 02291 02292 } 02293 02294 void calculaCarNCar(unsigned int ContadorEnsayos, unsigned int ContadorDatos, int PasoTiempo) 02295 { 02296 float t; 02297 float dZ,vZ,aZfilter10Hz,jerk,step2; 02298 02299 const float timeStep = (float)(PasoTiempo/1000000.0); 02300 const float Atadq = timeStep; 02301 02302 //Archivos de lectura y escritura 02303 FILE * fp; 02304 02305 char NoViajel[2]; 02306 char dispVelFile[50]; 02307 02308 strcpy(dispVelFile,"/local/dvaj2_"); 02309 sprintf (NoViajel, "%d", ContadorEnsayos); 02310 strcat(dispVelFile,NoViajel); 02311 strcat(dispVelFile,".txt"); 02312 02313 fp = fopen(dispVelFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", dispVelFile); } 02314 02315 int control(0); 02316 vector <int> regiones; 02317 02318 int nDatos = (ContadorDatos-(ndes+nmed)); 02319 if (nDatos < 0) {nDatos = 0;} 02320 02321 for(int i = 0; i<nDatos; i++) 02322 { 02323 02324 fscanf(fp,"%f %f %f %f %f %f \n", &t, &dZ, &vZ, &aZfilter10Hz, &jerk, &step2);//aZ filtered, aX y aY raw. 02325 02326 //Step 3: Identifica slopes con valor mayor que 0.3 m/s^3 02327 if ( (i >= Bo0) && (i <= Bo3) ) 02328 { 02329 if ((fabs(step2)>=0.3)&&(control==0)) 02330 { 02331 02332 regiones.push_back(i-((int)(0.5/timeStep)));//correccion indice por retraso step2 en 0.5 segundos OJO 02333 // regiones.push_back(i);//correccion indice por retraso step2 en 0.5 segundos OJO 02334 control=1; 02335 } 02336 else if ((fabs(step2)<=0.3)&&(control==1)) 02337 { 02338 control=0; 02339 regiones.push_back(i-1-((int)(0.5/timeStep)));//correccion indice por retraso step2 en 0.5 segundos 02340 // regiones.push_back(i-1);//correccion indice por retraso step2 en 0.5 segundos 02341 } 02342 } 02343 }//fin loop AQUI---abajo, cambiar time.at(regiones.at(2*i)) por tiempo sabiendo el paso etc. 02344 02345 //Step 4: Add 0.5 s . Hecho en el bucle de abajo. al primero le resto 0.5 y 02346 // al segundo se lo sumo. Para los indices como conozco Atadq, sumo y resto 02347 // 0.5/Atadq 02348 Nregiones = (int)(regiones.size()/2.0); 02349 //region [t1 t2 index(t1) index(t1)] 02350 02351 if(Nregiones > 0) 02352 { 02353 region = new float* [Nregiones]; 02354 for(unsigned int i = 0; i < Nregiones; i++) 02355 region[i] = new float[4]; 02356 02357 02358 for (unsigned int i = 0; i < Nregiones; i++) 02359 { 02360 float factorT(0.5); 02361 // while ( ((timeStep*regiones.at(2*i)-factorT)<0) || ((timeStep*regiones.at(2*i +1)+factorT)>time.at(time.size()-1)) ) 02362 while ( ((timeStep*regiones.at(2*i)-factorT)<0) || ((timeStep*regiones.at(2*i +1)+factorT)>timeStep*Bo3) ) 02363 { 02364 factorT = factorT - 0.1; 02365 } 02366 region[i][0] = timeStep*regiones.at(2*i)-factorT; 02367 region[i][2] = regiones.at(2*i)-factorT/Atadq; 02368 02369 region[i][1] = timeStep*regiones.at(2*i +1)+factorT; 02370 region[i][3] = regiones.at(2*i + 1)+factorT/Atadq; 02371 } 02372 // Step 5: cada fila de region (=Nregiones) es una region de non-constant 02373 // acceleration 02374 // Step 6: define regiones de constant acceleration (car) 02375 02376 int cont = 0; 02377 02378 for (unsigned int i=0; i < (Nregiones-1); i++) 02379 { 02380 if ( region[i+1][2] > region[i][3] ) 02381 { 02382 02383 regionconst[cont][0]=region[i][1]; 02384 regionconst[cont][1]=region[i+1][0]; 02385 regionconst[cont][2]=region[i][3]; 02386 regionconst[cont][3]=region[i+1][2]; 02387 cont++; 02388 } 02389 } 02390 Nregionesconst = cont; 02391 } 02392 else 02393 { 02394 Nregionesconst = 0; 02395 } 02396 02397 fclose(fp); 02398 int rmDVA0 = remove(dispVelFile); 02399 02400 } 02401 02402 void calculaZXYvibweighted( unsigned int ContadorEnsayos, unsigned int ContadorDatos) 02403 { 02404 //aux data 02405 float Zvibweighted_max(0.0); 02406 float Zvibweighted_min(0.0); 02407 float Zvibweighted_max_ncar(0.0); 02408 float Zvibweighted_min_ncar(0.0); 02409 float Xvibweighted_max(0.0); 02410 float Xvibweighted_min(0.0); 02411 float Yvibweighted_max(0.0); 02412 float Yvibweighted_min(0.0); 02413 int signoCompara; 02414 int signoCompara_ncar; 02415 int signoCompara_X; 02416 int signoCompara_Y; 02417 int signoControl; 02418 int signoControl_X; 02419 int signoControl_Y; 02420 int contadorCeros(0); 02421 int contadorCeros_ncar(0); 02422 int contadorCeros_X(0); 02423 int contadorCeros_Y(0); 02424 02425 02426 float difZ(0.0); 02427 float difX(0.0); 02428 float difY(0.0); 02429 int contador_peaktopeak_car(0); 02430 int contador_peaktopeak_X(0); 02431 int contador_peaktopeak_Y(0); 02432 02433 int marca(-1);//-1 = no 02434 02435 //read data from file 02436 float Xvibweighted; 02437 float Yvibweighted; 02438 float Zvibweighted; 02439 float t; 02440 02441 02442 //Archivos de lectura y escritura 02443 FILE * fp; 02444 FILE * fp2; 02445 FILE * fp3; 02446 FILE * fp4; 02447 02448 char NoViajel[2]; 02449 char dispVelFile[50]; 02450 char A95file[50]; 02451 char A95fileX[50]; 02452 char A95fileY[50]; 02453 02454 strcpy(dispVelFile,"/local/W5_IV_"); 02455 strcpy(A95file,"/local/A95_IV"); 02456 strcpy(A95fileX,"/local/AX_IV_"); 02457 strcpy(A95fileY,"/local/AY_IV_"); 02458 sprintf (NoViajel, "%d", ContadorEnsayos); 02459 strcat(dispVelFile,NoViajel); 02460 strcat(A95file,NoViajel); 02461 strcat(A95fileX,NoViajel); 02462 strcat(A95fileY,NoViajel); 02463 strcat(dispVelFile,".txt"); 02464 strcat(A95file,".txt"); 02465 strcat(A95fileX,".txt"); 02466 strcat(A95fileY,".txt"); 02467 02468 02469 fp = fopen(dispVelFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", dispVelFile); } 02470 fp2 = fopen(A95file,"w"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95file); } 02471 fp3 = fopen(A95fileX,"w"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileX); } 02472 fp4 = fopen(A95fileY,"w"); if(fp4 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileY); } 02473 02474 int nDatos = (ContadorDatos-(ndes+nmed)); 02475 if (nDatos < 0) {nDatos = 0;} 02476 02477 for(unsigned int i = 0; i<nDatos; i++)//-------------------------------------------------------------MAIN LOOP 02478 { 02479 fscanf(fp,"%f %f %f %f", &t, &Xvibweighted, &Yvibweighted, &Zvibweighted);//lee tiempo y 3 aceleraciones ponderadas X, Y, Z 02480 02481 /////////////////////////////////////////////////////////////////////////////////////////////// 02482 ////////////////Zvib/////////////////////////////////////////////////////////////////////////// 02483 /////////////////////////////////////////////////////////////////////////////////////////////// 02484 //signo 02485 signoControl = (int)(Zvibweighted/fabs(Zvibweighted));//+1 positivo, -1 negativo 02486 02487 ////////////////////////////////////////////////////////////////////////////////////// 02488 //Constant acceleration Region (subindex=car). Maximum peak to peak y A95 peak to peak 02489 //comprueba que esta dentro de alguna de las fronteras 02490 if ( Nregionesconst == 0 ) 02491 { 02492 max_peaktopeak_car_ = 0.0; 02493 A95_peaktopeak_car_ = 0.0; 02494 }else 02495 { 02496 for (int j = 0 ; j < Nregionesconst ; j++) 02497 { 02498 if ( i == ((int)regionconst[j][2]) ) 02499 { 02500 signoCompara = (int)(Zvibweighted/fabs(Zvibweighted));//+1 positivo, -1 negativo 02501 } 02502 if ( (((int)regionconst[j][2]) < i) && (i < (int)regionconst[j][3]) ) 02503 { 02504 if ( (signoControl-signoCompara) != 0 ) 02505 { 02506 contadorCeros++; 02507 signoCompara = signoControl; 02508 } 02509 if ( (1 <= contadorCeros) && (contadorCeros < 3) ) 02510 { 02511 if ( Zvibweighted > Zvibweighted_max ) 02512 { 02513 Zvibweighted_max = Zvibweighted; 02514 } 02515 if ( Zvibweighted < Zvibweighted_min ) 02516 { 02517 Zvibweighted_min = Zvibweighted; 02518 } 02519 }else if (contadorCeros == 3) 02520 { 02521 contadorCeros = 0; 02522 difZ = (Zvibweighted_max - Zvibweighted_min); 02523 if ( difZ > max_peaktopeak_car_ ){max_peaktopeak_car_ = difZ;} 02524 //almacena para A95 02525 fprintf(fp2,"%f \n", difZ); 02526 02527 contador_peaktopeak_car++; 02528 Zvibweighted_max = 0.0; 02529 Zvibweighted_min = 0.0; 02530 } 02531 } 02532 }//for Nregionesconst 02533 }//else 02534 02535 02536 ////////////////////////////////////////////////////////////////////////////////////// 02537 //Non-constant acceleration Region (subindex=ncar). Maximum peak to peak 02538 //comprueba que esta dentro de alguna de las fronteras 02539 if ( Nregiones == 0 ) 02540 { 02541 max_peaktopeak_ncar_ = 0.0; 02542 }else 02543 { 02544 for (int j = 0 ; j < Nregiones ; j++) 02545 { 02546 if ( i == ((int)region[j][2]) ) 02547 { 02548 signoCompara_ncar = (int)(Zvibweighted/fabs(Zvibweighted));//+1 positivo, -1 negativo 02549 marca = j; 02550 } 02551 if ( (((int)region[j][2]) < i) && (i < (int)region[j][3]) ) 02552 { 02553 if ( (signoControl-signoCompara_ncar) != 0 ) 02554 { 02555 contadorCeros_ncar++; 02556 signoCompara_ncar = signoControl; 02557 } 02558 if ( (1 <= contadorCeros_ncar) && (contadorCeros_ncar < 3))// && (marca == j) ) 02559 { 02560 if ( (Zvibweighted > Zvibweighted_max_ncar) && (marca == j) ) 02561 { 02562 Zvibweighted_max_ncar = Zvibweighted; 02563 } 02564 if ( (Zvibweighted < Zvibweighted_min_ncar) && (marca == j) ) 02565 { 02566 Zvibweighted_min_ncar = Zvibweighted; 02567 } 02568 }else 02569 { 02570 contadorCeros_ncar = 0; //marca = -1; 02571 if ( (Zvibweighted_max_ncar - Zvibweighted_min_ncar) > max_peaktopeak_ncar_ ){max_peaktopeak_ncar_ = (Zvibweighted_max_ncar - Zvibweighted_min_ncar);} 02572 Zvibweighted_max_ncar = 0.0; 02573 Zvibweighted_min_ncar = 0.0; 02574 } 02575 } 02576 }//for Nregiones 02577 }//else 02578 02579 ////////////////////////////////////////////////////////////////////////////////// 02580 ////////Xvib///////////////////////////////////////////////////////////////// 02581 ////////////////////////////////////////////////////////////////////////////////// 02582 //signo 02583 signoControl_X = (int)(Xvibweighted/fabs(Xvibweighted));//+1 positivo, -1 negativo 02584 if ( i == Bo1 ) 02585 { 02586 signoCompara_X = (int)(Xvibweighted/fabs(Xvibweighted));//+1 positivo, -1 negativo 02587 } 02588 if ( Bo1 == Bo2 ) 02589 { 02590 max_peaktopeak_X_ = 0.0; 02591 }else 02592 { 02593 if (( Bo1 < i) && (i < Bo2 )) 02594 { 02595 if ( (signoControl_X-signoCompara_X) != 0 ) 02596 { 02597 contadorCeros_X++; 02598 signoCompara_X = signoControl_X; 02599 } 02600 if ( (1 <= contadorCeros_X) && (contadorCeros_X < 3) ) 02601 { 02602 if ( Xvibweighted > Xvibweighted_max ) 02603 { 02604 Xvibweighted_max = Xvibweighted; 02605 } 02606 if ( Xvibweighted < Xvibweighted_min ) 02607 { 02608 Xvibweighted_min = Xvibweighted; 02609 } 02610 }else if (contadorCeros_X == 3) 02611 { 02612 contadorCeros_X = 0; 02613 difX = (Xvibweighted_max - Xvibweighted_min); 02614 if ( difX > max_peaktopeak_X_ ){max_peaktopeak_X_ = difX;} 02615 //almacena para A95 02616 fprintf(fp3,"%f \n", difX); 02617 contador_peaktopeak_X++; 02618 Xvibweighted_max = 0.0; 02619 Xvibweighted_min = 0.0; 02620 } 02621 } 02622 } 02623 02624 ////////////////////////////////////////////////////////////////////////////////// 02625 ////////Yvib///////////////////////////////////////////////////////////////// 02626 ////////////////////////////////////////////////////////////////////////////////// 02627 //signo 02628 signoControl_Y = (int)(Yvibweighted/fabs(Yvibweighted));//+1 positivo, -1 negativo 02629 if ( i == Bo1 ) 02630 { 02631 signoCompara_Y = (int)(Yvibweighted/fabs(Yvibweighted));//+1 positivo, -1 negativo 02632 } 02633 if ( Bo1 == Bo2 ) 02634 { 02635 max_peaktopeak_Y_ = 0.0; 02636 }else 02637 { 02638 if (( Bo1 < i) && (i < Bo2 )) 02639 { 02640 if ( (signoControl_Y-signoCompara_Y) != 0 ) 02641 { 02642 contadorCeros_Y++; 02643 signoCompara_Y = signoControl_Y; 02644 } 02645 if ( (1 <= contadorCeros_Y) && (contadorCeros_Y < 3) ) 02646 { 02647 if ( Yvibweighted > Yvibweighted_max ) 02648 { 02649 Yvibweighted_max = Yvibweighted; 02650 } 02651 if ( Yvibweighted < Yvibweighted_min ) 02652 { 02653 Yvibweighted_min = Yvibweighted; 02654 } 02655 }else if (contadorCeros_Y == 3) 02656 { 02657 contadorCeros_Y = 0; 02658 difY = (Yvibweighted_max - Yvibweighted_min); 02659 if ( difY > max_peaktopeak_Y_ ){max_peaktopeak_Y_ = difY;} 02660 //almacena para A95 02661 fprintf(fp4,"%f \n", difY); 02662 contador_peaktopeak_Y++; 02663 Yvibweighted_max = 0.0; 02664 Yvibweighted_min = 0.0; 02665 } 02666 } 02667 } 02668 02669 }//fin Loop Principal 02670 02671 02672 fclose(fp); 02673 if ( fp2 != NULL ) 02674 { 02675 fclose(fp2); 02676 } 02677 if ( fp3 != NULL ) 02678 { 02679 fclose(fp3); 02680 } 02681 if ( fp4 != NULL ) 02682 { 02683 fclose(fp4); 02684 } 02685 02686 02687 ///////////////////////////////////////////////////////////// 02688 //Calculo A95s/////////////////////////////////////////////// 02689 //Z 02690 //abro fichero 02691 02692 fp2 = fopen(A95file,"r"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95file); } 02693 float maxZvibweighted; 02694 int length_maximoZvib = (int)(contador_peaktopeak_car*0.05) + 1; 02695 int maxindex(0); 02696 vector<float> maximoZvib (length_maximoZvib,0.0); 02697 vector<float> diferencia_vec; 02698 vector<float>::iterator up; 02699 float diferencia; 02700 float maximo; 02701 int contadorA95(0); 02702 //pasada 1 02703 for(unsigned int i = 0; i<contador_peaktopeak_car; i++) 02704 { 02705 fscanf(fp2,"%f", &maxZvibweighted); 02706 for (unsigned int j = 0; j<length_maximoZvib; j++) 02707 { 02708 diferencia = maxZvibweighted - maximoZvib.at(j); 02709 if ( diferencia < 0 ) 02710 { 02711 diferencia_vec.push_back(0.0); 02712 contadorA95++; 02713 }else 02714 { 02715 diferencia_vec.push_back(diferencia); 02716 } 02717 } 02718 if ( contadorA95 == length_maximoZvib ){} 02719 else 02720 { 02721 maximo = *max_element(diferencia_vec.begin(),diferencia_vec.end()); 02722 up = find(diferencia_vec.begin(), diferencia_vec.end(), maximo); 02723 maxindex = int(up - diferencia_vec.begin()); 02724 maximoZvib.erase(maximoZvib.begin() + maxindex ); 02725 maximoZvib.insert(maximoZvib.begin() + maxindex ,maxZvibweighted); 02726 } 02727 contadorA95 = 0; 02728 diferencia_vec.clear(); 02729 } 02730 sort(maximoZvib.begin(),maximoZvib.end());//ascending 02731 reverse(maximoZvib.begin(),maximoZvib.end());//descending 02732 fclose(fp2); 02733 02734 02735 02736 //pasada 2 02737 //abro fichero 02738 fp2 = fopen(A95file,"r"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95file); } 02739 double resta(1000.0); 02740 for(unsigned int i = 0; i<contador_peaktopeak_car; i++) 02741 { 02742 fscanf(fp2,"%f", &maxZvibweighted); 02743 if( ((maximoZvib.at(maximoZvib.size() - 1) - maxZvibweighted) <= resta) && (maxZvibweighted < maximoZvib.at(maximoZvib.size() - 1)) ) 02744 { 02745 A95_peaktopeak_car_ = maxZvibweighted; 02746 resta = maximoZvib.at(maximoZvib.size() - 1) - maxZvibweighted; 02747 02748 } 02749 } 02750 //clear all vectors 02751 maximoZvib.clear(); 02752 diferencia_vec.clear(); 02753 02754 02755 02756 fclose(fp2); 02757 02758 //X///////////////////////////////////////// 02759 //abro fichero 02760 fp3 = fopen(A95fileX,"r"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileX); } 02761 float maxXvibweighted; 02762 int length_maximoXvib = (int)(contador_peaktopeak_X*0.05) + 1; 02763 maxindex = 0; 02764 vector<float> maximoXvib (length_maximoXvib,0.0); 02765 diferencia = 0.0; 02766 maximo =0.0; 02767 contadorA95 = 0; 02768 if ( fp3 == NULL ) 02769 { 02770 A95_peaktopeak_X_ = 0.0; 02771 }else 02772 { 02773 //pasada 1 02774 for(unsigned int i = 0; i<contador_peaktopeak_X; i++) 02775 { 02776 fscanf(fp3,"%f", &maxXvibweighted); 02777 for (unsigned int j = 0; j<length_maximoXvib; j++) 02778 { 02779 diferencia = maxXvibweighted - maximoXvib.at(j); 02780 if ( diferencia < 0 ) 02781 { 02782 diferencia_vec.push_back(0.0); 02783 contadorA95++; 02784 }else 02785 { 02786 diferencia_vec.push_back(diferencia); 02787 } 02788 } 02789 if ( contadorA95 == length_maximoXvib ){} 02790 else 02791 { 02792 maximo = *max_element(diferencia_vec.begin(),diferencia_vec.end()); 02793 up = find(diferencia_vec.begin(), diferencia_vec.end(), maximo); 02794 maxindex = int(up - diferencia_vec.begin()); 02795 maximoXvib.erase(maximoXvib.begin() + maxindex ); 02796 maximoXvib.insert(maximoXvib.begin() + maxindex ,maxXvibweighted); 02797 } 02798 contadorA95 = 0; 02799 diferencia_vec.clear(); 02800 } 02801 sort(maximoXvib.begin(),maximoXvib.end());//ascending 02802 reverse(maximoXvib.begin(),maximoXvib.end());//descending 02803 fclose(fp3); 02804 02805 //pasada 2 02806 //abro fichero 02807 fp3 = fopen(A95fileX,"r"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileX); } 02808 resta = 1000.0; 02809 for(unsigned int i = 0; i<contador_peaktopeak_X; i++) 02810 { 02811 fscanf(fp3,"%f", &maxXvibweighted); 02812 if( ((maximoXvib.at(maximoXvib.size() - 1) - maxXvibweighted) <= resta) && (maxXvibweighted < maximoXvib.at(maximoXvib.size() - 1)) ) 02813 { 02814 A95_peaktopeak_X_ = maxXvibweighted; 02815 resta = maximoXvib.at(maximoXvib.size() - 1) - maxXvibweighted; 02816 02817 } 02818 } 02819 //clear all vectors 02820 maximoXvib.clear(); 02821 diferencia_vec.clear(); 02822 } 02823 02824 02825 02826 fclose(fp3); 02827 02828 //Y///////////////////////////////////////// 02829 //abro fichero 02830 fp4 = fopen(A95fileY,"r"); if(fp4 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileY); } 02831 float maxYvibweighted; 02832 int length_maximoYvib = (int)(contador_peaktopeak_Y*0.05) + 1; 02833 maxindex = 0; 02834 vector<float> maximoYvib (length_maximoYvib,0.0); 02835 diferencia = 0.0; 02836 maximo =0.0; 02837 contadorA95 = 0; 02838 if ( fp4 == NULL ) 02839 { 02840 A95_peaktopeak_Y_ = 0.0; 02841 }else 02842 { 02843 //pasada 1 02844 for(unsigned int i = 0; i<contador_peaktopeak_Y; i++) 02845 { 02846 fscanf(fp4,"%f", &maxYvibweighted); 02847 for (unsigned int j = 0; j<length_maximoYvib; j++) 02848 { 02849 diferencia = maxYvibweighted - maximoYvib.at(j); 02850 if ( diferencia < 0 ) 02851 { 02852 diferencia_vec.push_back(0.0); 02853 contadorA95++; 02854 }else 02855 { 02856 diferencia_vec.push_back(diferencia); 02857 } 02858 } 02859 if ( contadorA95 == length_maximoYvib ){} 02860 else 02861 { 02862 maximo = *max_element(diferencia_vec.begin(),diferencia_vec.end()); 02863 up = find(diferencia_vec.begin(), diferencia_vec.end(), maximo); 02864 maxindex = int(up - diferencia_vec.begin()); 02865 maximoYvib.erase(maximoYvib.begin() + maxindex ); 02866 maximoYvib.insert(maximoYvib.begin() + maxindex ,maxYvibweighted); 02867 } 02868 contadorA95 = 0; 02869 diferencia_vec.clear(); 02870 } 02871 sort(maximoYvib.begin(),maximoYvib.end());//ascending 02872 reverse(maximoYvib.begin(),maximoYvib.end());//descending 02873 fclose(fp4); 02874 02875 //pasada 2 02876 //abro fichero 02877 fp4 = fopen(A95fileY,"r"); if(fp4 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileY); } 02878 resta = 1000.0; 02879 for(unsigned int i = 0; i<contador_peaktopeak_Y; i++) 02880 { 02881 fscanf(fp4,"%f", &maxYvibweighted); 02882 if( ((maximoYvib.at(maximoYvib.size() - 1) - maxYvibweighted) <= resta) && (maxYvibweighted < maximoYvib.at(maximoYvib.size() - 1)) ) 02883 { 02884 A95_peaktopeak_Y_ = maxYvibweighted; 02885 resta = maximoYvib.at(maximoYvib.size() - 1) - maxYvibweighted; 02886 02887 } 02888 } 02889 //clear all vectors 02890 maximoYvib.clear(); 02891 diferencia_vec.clear(); 02892 } 02893 02894 02895 fclose(fp4); 02896 //remove files 02897 int rmDVA0 = remove(A95file); if(rmDVA0 != 0) { display.Clear();display.SetXY(0,0); display.printf("Error removing %s file\n", A95file); } 02898 02899 int rmDVA1 = remove(A95fileX); if(rmDVA1 != 0) { display.Clear();display.SetXY(0,0); display.printf("Error removing %s file\n", A95fileX); } 02900 int rmDVA2 = remove(A95fileY); if(rmDVA2 != 0) { display.Clear();display.SetXY(0,0); display.printf("Error removing %s file\n", A95fileY); } 02901 int rmDVA3 = remove(dispVelFile); if(rmDVA3 != 0) { display.Clear();display.SetXY(0,0); display.printf("Error removing %s file\n", dispVelFile); } 02902 02903 02904 02905 02906 02907 02908 02909 02910 return; 02911 02912 } 02913 02914 float RegresionJerk(float * aZ, unsigned int ndat) 02915 { 02916 //Media de las aceleraciones 02917 float aZm = 0.0; 02918 for(unsigned int j = 0; j<ndat; j++) { aZm += aZ[j]; } 02919 aZm = aZm/(float)ndat; 02920 02921 //Pendiente por minimos cuadrados 02922 float b; 02923 //Numerador y denominador en expresion de pendiente 02924 float numer = 0.0; 02925 float denom = 0.0; 02926 02927 int nn; 02928 float At; 02929 02930 for(int j = 0; j<ndat; j++) 02931 { 02932 nn = j-((ndat/2)); 02933 At = (float)nn*(1.0e-6)*(float)(PasoTiempo); 02934 denom = denom + At*At; 02935 numer = numer + At*(aZ[j]-aZm); 02936 } 02937 b = numer/denom; 02938 02939 return b; 02940 } 02941 02942 void DisplayResultados1(void) 02943 { 02944 for(int i = 0; i<2; i++) 02945 { 02946 02947 display.Clear(); 02948 if(TRADUCE==0) 02949 { 02950 display.SetXY(0,0); 02951 display.printf("Datos velocidad"); 02952 display.SetXY(0,1); 02953 display.printf("aceleracion jerk"); 02954 }else 02955 { 02956 display.SetXY(0,0); 02957 display.printf("Velocity jerk"); 02958 display.SetXY(0,1); 02959 display.printf("acceleration"); 02960 } 02961 02962 wait(2.0); 02963 02964 display.Clear(); 02965 display.SetXY(0,0); 02966 display.printf("VNom = %4.2f m/s \n",vNom); 02967 display.SetXY(0,1); 02968 display.printf("Vmax = %4.2f m/s \n", Vmax); 02969 02970 wait(2.0); 02971 02972 display.Clear(); 02973 display.SetXY(0,0); 02974 display.printf("amax = %4.2f m/s2\n", amax); 02975 display.SetXY(0,1); 02976 display.printf("dmax = %4.2f m/s2\n", dmax); 02977 02978 wait(2.0); 02979 02980 display.Clear(); 02981 display.SetXY(0,0); 02982 display.printf("jmax = %4.2f m/s3\n", jerkmax); 02983 display.SetXY(0,1); 02984 display.printf("VA95 = %4.2f m/s \n", VA95); 02985 02986 wait(2.0); 02987 02988 display.Clear(); 02989 display.SetXY(0,0); 02990 display.printf("aA95 = %4.2f m/s2\n", aA95); 02991 display.SetXY(0,1); 02992 display.printf("dA95 = %4.2f m/s2\n", dA95); 02993 02994 wait(2.0); 02995 02996 } 02997 } 02998 02999 void DisplayResultados2(void) 03000 { 03001 for(int i = 0; i<2; i++) 03002 { 03003 03004 display.Clear(); 03005 if(TRADUCE==0) 03006 { 03007 display.SetXY(0,0); 03008 display.printf("Datos confort"); 03009 display.SetXY(0,0); 03010 display.printf("vibracion Z,X,Y:"); 03011 }else 03012 { 03013 display.SetXY(0,0); 03014 display.printf("Comfort info"); 03015 display.SetXY(0,0); 03016 display.printf("vibration Z,X,Y:"); 03017 } 03018 wait(2.0); 03019 03020 03021 03022 display.Clear(); 03023 display.SetXY(0,0); 03024 display.printf("Zmaxca=%4.2f m/s2 \n", max_peaktopeak_car_); 03025 display.SetXY(0,1); 03026 display.printf("ZA95ca=%4.2f m/s2 \n", A95_peaktopeak_car_); 03027 03028 wait(2.0); 03029 03030 display.Clear(); 03031 display.SetXY(0,0); 03032 display.printf("Zmaxnca=%4.2f m/s2 \n", max_peaktopeak_ncar_); 03033 03034 wait(2.0); 03035 03036 display.Clear(); 03037 display.SetXY(0,0); 03038 display.printf("Xmax = %4.2f m/s2\n", max_peaktopeak_X_); 03039 display.SetXY(0,1); 03040 display.printf("XA95 = %4.2f m/s2\n", A95_peaktopeak_X_); 03041 03042 wait(2.0); 03043 03044 display.Clear(); 03045 display.SetXY(0,0); 03046 display.printf("Ymax = %4.2f m/s2\n", max_peaktopeak_Y_); 03047 display.SetXY(0,1); 03048 display.printf("YA95 = %4.2f m/s2\n", A95_peaktopeak_Y_); 03049 03050 wait(2.0); 03051 } 03052 } 03053 03054 void FileResultados( unsigned int ContadorEnsayos ) 03055 { 03056 FILE * fp; 03057 03058 char NoViaje[2]; 03059 char resultsFile[50]; 03060 03061 strcpy(resultsFile,"/local/ISO_"); 03062 03063 sprintf (NoViaje, "%d", ContadorEnsayos); 03064 strcat(resultsFile,NoViaje); 03065 strcat(resultsFile,".txt"); 03066 03067 03068 fp = fopen(resultsFile,"w"); if(fp == NULL) { display.printf("Could not open file %s for read\n", resultsFile); } 03069 fprintf(fp,"%f %f %f %f %f %f %f %f %f %f %f %f %f %f \n", amax, dmax, aA95, dA95, jerkmax, Vmax, VA95, max_peaktopeak_car_, A95_peaktopeak_car_, max_peaktopeak_ncar_, max_peaktopeak_X_, A95_peaktopeak_X_, max_peaktopeak_Y_, A95_peaktopeak_Y_); 03070 //fprintf(fp,"%d %d %d \n", Bo1, Bo2, Bo3); 03071 fclose(fp); 03072 03073 } 03074 03075 void LeeAcelerometroBuffer() 03076 { 03077 bma180.ReadAccels_BMA180(readings); 03078 03079 aaX = (int16_t)readings[0]; 03080 aaY = (int16_t)readings[1]; 03081 aaZ = (int16_t)readings[2]; 03082 //tt_inst = (int16_t)timer.read_us(); 03083 03084 aX = (-1.0)*((float)aaX-(-103.23))*0.0018; 03085 aY = (-1.0)*((float)aaY-(57.21))*0.0018; 03086 aZ = (-1.0)*((float)aaZ-(701.1184))*0.0018097570403; 03087 03088 //t_inst = (float)tt_inst; 03089 03090 //buff_t.put(t_inst); 03091 buff_aX.put(aX); 03092 buff_aY.put(aY); 03093 buff_aZ.put(aZ); 03094 } 03095 03096 void GuardaDatosBuffer() 03097 { 03098 bool empty = buff_aX.isEmpty(); 03099 int wi = buff_aX.getWritingIndex(); 03100 int ri = buff_aX.getReadingIndex(); 03101 03102 int dif; //Diferencia entre writting index y riding index. El buffer se llena cuando esta diferencia alcanza el tamaño del buffer 03103 float tiempo; //Tiempo en segundos 03104 03105 if((wi-ri)>0) { dif = wi-ri; } 03106 else { dif = sizeBuff+wi-ri; } 03107 03108 if(empty == false) 03109 { 03110 aXr = buff_aX.get(); 03111 aYr = buff_aY.get(); 03112 aZr = buff_aZ.get(); 03113 03114 tiempo = ((float)ContadorDatos)*((float)PasoTiempo)/1.0e6; 03115 03116 //pc.printf("%d %f %f %f %d\n", ContadorDatos*PasoTiempo, aXr, aYr, aZr, dif); 03117 fprintf(fp,"%f %f %f %f %d\n", tiempo, aXr, aYr, aZr, dif); 03118 ContadorDatos++; 03119 } 03120 } 03121 03122 void ParamDependFrecAdq() 03123 { 03124 03125 if(FrecAdq == 320) 03126 { 03127 PasoTiempo = 3125; 03128 gain = 1.184456202e+02; 03129 c_butt1 = -0.7575469445; 03130 c_butt2 = 1.7237761728; 03131 ndes = 100; 03132 nmed = 180; 03133 } 03134 03135 if(FrecAdq == 1000) 03136 { 03137 PasoTiempo = 1000; 03138 gain = 1.058546241e+03; 03139 c_butt1 = -0.9149758348; 03140 c_butt2 = 1.9111970674; 03141 ndes = 300; 03142 nmed = 600; 03143 } 03144 03145 /// Para fadq = 100 Hz, PasoTiempo = 10000 /// 03146 03147 //float gain = 1.482463775e+01; 03148 //float c_butt1 = -0.4128015981; 03149 //float c_butt2 = 1.1429805025; 03150 //unsigned int ndes = 30; 03151 //unsigned int nmed = 60; 03152 03153 /// Para fadq = 328 Hz, PasoTiempo = 3050 /// 03154 03155 //float gain = 1.239670776e+02; 03156 //float c_butt1 = -0.7626121550; 03157 //float c_butt2 = 1.7303455236; 03158 //unsigned int ndes = 100; 03159 //unsigned int nmed = 180; 03160 03161 /// Para fadq = 320 Hz, PasoTiempo = 3125 /// 03162 03163 //const float gain = 1.184456202e+02; 03164 //const float c_butt1 = -0.7575469445; 03165 //const float c_butt2 = 1.7237761728; 03166 //unsigned int ndes = 100; 03167 //unsigned int nmed = 180; 03168 03169 /// Para fadq = 1000 Hz, PasoTiempo = 1000 /// 03170 03171 //const float gain = 1.058546241e+03; 03172 //const float c_butt1 = -0.9149758348; 03173 //const float c_butt2 = 1.9111970674; 03174 //const unsigned int ndes = 300; 03175 //const unsigned int nmed = 600; 03176 }
Generated on Thu Jul 14 2022 18:41:53 by
1.7.2