Esta versión v6 pasa a ser el nuevo master. Funciona correctamente
Dependencies: ADXL345 Display1602 MSCFileSystem SDFileSystem mbed FATFileSystem
vmConfort.cpp
- Committer:
- jjmedina
- Date:
- 2014-05-20
- Revision:
- 0:a5367bd4e404
- Child:
- 1:ff391cfd0e77
File content as of revision 0:a5367bd4e404:
#include "mbed.h" #include "Display1602.h" //#include "SDFileSystem.h" #include "MSCFileSystem.h" #include <iostream> #include <math.h> #include <stdio.h> #include <string.h> #include <list> #include <vector> #include <algorithm> #include "filters.h" #include "BMA180.h" //#include "ADXL345.h" #include "RingBuffer.h" using namespace std; Timer timer; int TiempoInicio = 0; int TiempoFin = 0; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /// CONSTANTES DEPENDIENTES DE LA FRECUENCIA DE ADQUISICION (sus valores se establecen en ParamDependFrecAdq()) /// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// unsigned int PasoTiempo; //Paso de tiempo de adquisicion en [us]. Frecuencia de adquisicion = 1/PasoTiemo float gain; // CONSTANTE DEL FILTRO DE BUTTERWORTH PASO BAJO 10 HZ float c_butt1; // CONSTANTE DEL FILTRO DE BUTTERWORTH PASO BAJO 10 HZ float c_butt2; // CONSTANTE DEL FILTRO DE BUTTERWORTH PASO BAJO 10 HZ unsigned int ndes; //Puntos que se descartan al principio por el Filtro Butterworth unsigned int nmed; //Puntos para calcular la media de aZ al principio del viaje despues de los puntos descartados int nDatosPorPaso = 1; ///////////////////////////////// /// DISPOSITIVOS, PERIFERICOS /// ///////////////////////////////// /// ACELEROMETRO ANALOGICO /// //AnalogIn Pin_aX(p15); //AnalogIn Pin_aY(p16); //AnalogIn Pin_aZ(p17); /// ACELEROMETRO DIGITAL Bosch/// BMA180 bma180(p5,p6,p7,p8); /// ACELEROMETRO DIGITAL AnalogDevices ADXL345/// //ADXL345 adxl345(p5, p6, p7, p8); Serial pc(USBTX, USBRX); // tx, rx /// TARJETA SD /// //SDFileSystem sd(p11, p12, p13, p14, "local"); /// PEN DRIVE /// MSCFileSystem msc("local"); /// MEMORIA FLASH LOCAL DE LA MBED /// LocalFileSystem local("mbedFlash"); // Create the local filesystem under the name "local" /// LEDS /// DigitalOut red_LED(p30); DigitalOut green_LED(p29); /// PUSH BUTTONS /// DigitalOut red_PUSH(p9); DigitalOut green_PUSH(p10); /// DISPLAY LCD /// //Display1602 display(p18,p19,p20,p28,p27,p26,p25,p24,p23,p22,p21); Display1602 display(p18,p19,p20,p21,p22,p23,p24,p25,p26,p27,p28); //void InterpolaMedidasPerdidas(unsigned int, unsigned int); void FiltradoPeriodosIntegracion(unsigned int, unsigned int); void DatosConfort(unsigned int, unsigned int, int); float RegresionJerk(float *, unsigned int); void calcula_n05n95_Boveloc_jerkmax(unsigned int , unsigned int, int ); void calcula_A95_V_acel_decel(unsigned int, unsigned int, int); void vibraReadFilter(unsigned int , unsigned int ); void calculaCarNCar(unsigned int , unsigned int, int ); void calculaStep2(unsigned int , unsigned int ); void calculaZXYvibweighted( unsigned int , unsigned int); void DisplayResultados1(void); void DisplayResultados2(void); void FileResultados( unsigned int ); unsigned int nArr, nPar, nV0, nVfin; //Instantes (orden en el vector de datos) caracteristicos del viaje float t_Arr, t_V0, t_Vfin, t_Par; //Instantes (valores del tiempo) caracteristicos del viaje float vArr = 0.0; //Velocidad que se calcula por integración en el instante de arranque (debe ser 0) float vPar = 0.0; //Velocidad que se calcula por integración en el instante de parada (debe ser 0) float vZmFin = 0.0; //Media de vZ al final del viaje (debe ser zero pero no lo es) float tFin; //Instante final float aZm0 = 0.0; //Aceleracion media de referencia en instantes iniciales en dirección vertical Z float aYm0 = 0.0; //Aceleracion media de referencia en instantes iniciales en dirección lateral X float aXm0 = 0.0; //Aceleracion media de referencia en instantes iniciales en dirección lateral Y ////////////////////////// /// MEDIDAS EN DISPLAY /// ////////////////////////// float vNom = 0.0; //Velocidad nominal float amax = 0.0; float dmax = 0.0; float Vmax = 0.0; float jerkmax = 0.0; float VA95(0.0); float aA95(0.0); float dA95(0.0); //por pantalla float max_peaktopeak_car_(0.0); float A95_peaktopeak_car_(0.0); float max_peaktopeak_ncar_(0.0); float max_peaktopeak_X_(0.0); float A95_peaktopeak_X_(0.0); float max_peaktopeak_Y_(0.0); float A95_peaktopeak_Y_(0.0); float d_arr, d_par; //Desplazamientos medidos en instantes de arranque y de parada unsigned int Bo0, Bo1, Bo2, Bo3; //Limites de los periodos de calculo de norma ISO unsigned int Boacel; //Instante donde deja de acelerar para empezar a decelerar float Vmax1 = 0.0; float Vmax2 = 0.0; unsigned int n05Vmax1(0);unsigned int n95Vmax1(0);unsigned int n05Vmax2(0);unsigned int n95Vmax2(0); unsigned int Bo_veloc_acel(0); unsigned int Bo_veloc_decel(0); int Nregionesconst(0); int Nregiones(0); float **region; float regionconst[20][4]; //50 porque no sabemos el numero de filas. RESOLVER MEJOR, JUAN 04-05-2012. //////////////////////// /// IDIOMA DE LA GUI /// //////////////////////// unsigned int TRADUCE = 0; // TRADUCE = 0 gui en español, TRADUCE = 1, traduce gui al inglés unsigned int SeleccionIdioma = 0; // Vale 1 cuando ya se ha seleccionado el idioma ///////////////////////////////// /// FRECUENCIA DE ADQUISICION /// ///////////////////////////////// unsigned int FrecAdq = 0; //La frecuencia de adquisición puede ser 320 Hz (FrecAdq = 320) o 1000 Hz (FrecAdq = 1000) unsigned int SeleccionFrecAdq = 0; // Vale 1 cuando ya se ha seleccionado la frecuencia de adquisicion ////////////////////////////////////////// /// ALMACENAMIENTO DE DATOS CON BUFFER /// ////////////////////////////////////////// void LeeAcelerometroBuffer(); void GuardaDatosBuffer(); int readings[3] = {0, 0, 0}; //Lectura aceleraciones sensor digital float aX, aY, aZ; //Medidas de aceleracion int16_t aaX, aaY, aaZ; //Medidas de aceleracion y tiempo float aXr, aYr, aZr; //Tiempo y medidas de aceleracion leidos del buffer unsigned int contDatos = 0; //Contador de datos int sizeBuff = 500; //Tamaño de los buffer que guardan los datos Ticker tickerLectura; Buffer buff_aX(sizeBuff); Buffer buff_aY(sizeBuff); Buffer buff_aZ(sizeBuff); FILE * fp; FILE * fp2; unsigned int ContadorEnsayos = 0; unsigned int ContadorDatos = 0; unsigned int ContadorMedidas = 0; void ParamDependFrecAdq(void); int main() { pc.baud(57600); timer.start(); bool datosPC_green = false; //true = envia datos a PC, false = Guarda datos en tarjeta SD bool ensayo_red = false; //true = Comienza la medida, false = termina la medida char NoViaje[2]; ////////////////////////////////////////////// /// ESTABLECE LOS OFFSETS DEL ACELEROMETRO /// ////////////////////////////////////////////// char offsets[6]; offsets[0]= 0xAE; //x_msb offsets[1]= 0x9E; //y_msb offsets[2]= 0x95; //z_msb offsets[3]= 0x0C; //x_lsb offsets[4]= 0x04; //y_lsb offsets[5]= 0x0C; //z_lsb //bma180.AcelerometroLeeOffsets(readings); //pc.printf("%d %d %d\n", readings[0], readings[1], readings[2]); //bma180.AcelerometroEscribeOffsets(offsets); //bma180.AcelerometroLeeOffsets(readings); //pc.printf("%d %d %d\n", readings[0], readings[1], readings[2]); ///////////////////////////// /// APAGA EL ACELEROMETRO /// ///////////////////////////// bma180.AcelerometroSleep(); ///////////////////////////////////// /// SELECCION DE IDIOMA DE LA GUI /// ///////////////////////////////////// display.Clear(); display.SetXY(0,0); display.printf("English: yellow"); display.SetXY(0,1); display.printf("Castellano: rojo"); while(SeleccionIdioma == 0) { if(red_PUSH.read() == 1) { TRADUCE = 0;//traduce gui al español SeleccionIdioma = 1; wait(0.5); } if(green_PUSH.read() == 1) { TRADUCE = 1;//traduce gui al ingles SeleccionIdioma = 1; wait(0.5); } } ///////////////////////////////////////////////// /// SELECCION DE LA FRECUENCIA DE ADQUISICION /// ///////////////////////////////////////////////// if(TRADUCE == 0) { display.Clear(); display.SetXY(0,0); display.printf("320 Hz: amarillo"); display.SetXY(0,1); display.printf("1000 Hz: rojo"); } if(TRADUCE == 1) { display.Clear(); display.SetXY(0,0); display.printf("320 Hz: yellow"); display.SetXY(0,1); display.printf("1000 Hz: red"); } while(SeleccionFrecAdq == 0) { if(red_PUSH.read() == 1) { FrecAdq = 1000; SeleccionFrecAdq = 1; wait(0.5); } if(green_PUSH.read() == 1) { FrecAdq = 320; SeleccionFrecAdq = 1; wait(0.5); } } ParamDependFrecAdq(); //Establece el valor de los parámetros que depeneden de la frecuencia de adquisicion //////////////////////////////////////////////// /// AVISA MODO FUNCIONAMIENTO: PC O AUTONOMO /// //////////////////////////////////////////////// if(datosPC_green == true) { green_LED = 1; if(TRADUCE == 0) { display.Clear(); display.SetXY(0,0); display.printf("Aparato en"); display.SetXY(0,1); display.printf("modo PC."); wait(1.5); display.Clear(); display.SetXY(0,0); display.printf("Para cambio a"); display.SetXY(0,1); display.printf("modo autonomo..."); wait(1.5); display.Clear(); display.SetXY(0,0); display.printf("...pulse boton"); display.SetXY(0,1); display.printf("amarillo."); wait(1.5); display.Clear(); } else { display.Clear(); display.SetXY(0,0); display.printf("Set in PC mode."); wait(1.5); display.Clear(); display.SetXY(0,0); display.printf("To change to"); display.SetXY(0,1); display.printf("autonomous mode"); wait(1.5); display.Clear(); display.SetXY(0,0); display.printf("...press"); display.SetXY(0,1); display.printf("yellow button."); wait(1.5); display.Clear(); } } else { green_LED = 0; if(TRADUCE == 0) { display.Clear(); display.SetXY(0,0); display.printf("Aparato en"); display.SetXY(0,1); display.printf("modo autonomo."); wait(1.5); display.Clear(); display.SetXY(0,0); display.printf("Para cambio a"); display.SetXY(0,1); display.printf("modo PC..."); wait(1.5); display.Clear(); display.SetXY(0,0); display.printf("...pulse boton"); display.SetXY(0,1); display.printf("amarillo."); wait(1.5); display.Clear(); } else { display.Clear(); display.SetXY(0,0); display.printf("Set in "); display.SetXY(0,1); display.printf("autonomous mode."); wait(1.5); display.Clear(); display.SetXY(0,0); display.printf("To change to"); display.SetXY(0,1); display.printf("PC mode..."); wait(1.5); display.Clear(); display.SetXY(0,0); display.printf("...press"); display.SetXY(0,1); display.printf("yellow button."); wait(1.5); display.Clear(); } } while((SeleccionIdioma == 1)&&(SeleccionFrecAdq == 1)) { //////////////////////////////////////// /// INVITA A COMENZAR LA ADQUISICION /// //////////////////////////////////////// if(ensayo_red == false) { if(TRADUCE == 0) { display.Clear(); display.SetXY(0,0); display.printf("Pulse rojo"); display.SetXY(0,1); display.printf("para empezar"); wait(0.1); } if(TRADUCE == 1) { display.Clear(); display.SetXY(0,0); display.printf("Push red"); display.SetXY(0,1); display.printf("to start"); wait(0.1); } } /////////////////////////////// // COMIENZO Y FIN DE MEDIDDA // /////////////////////////////// if(red_PUSH.read() == 1) //si pulsa el rojo (o empieza o acaba medidas.) { if(ensayo_red == false)//antes de pulsar estaba apagado, por tanto empieza medidas. { ensayo_red = true;//lo enciende //////////////////////////////// /// ENCIENDE EL ACELEROMETRO /// //////////////////////////////// bma180.AcelerometroWakeUp(); 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) { // LEE DE FICHERO EL ORDEN DEL ENSAYO ANTERIOR Y VUELVE A GUARDAR EL ORDEN INCREMENTADO EN 1 fp2 = fopen("/mbedFlash/OrdEns.txt","r"); if(fp2 == NULL) { //SI NO EXISTE EL ARCHIVO CON EL ORDEN DEL ENSAYO SE CREA Y SE PONE UN 0 pc.printf("Could not open file OrdEns.txt for read\n"); fp2 = fopen("/mbedFlash/OrdEns.txt","w"); if(fp2 == NULL) { pc.printf("Could not open file OrdEns.txt for write\n"); } fprintf(fp2,"0"); fclose(fp2); } fscanf(fp2,"%d", &ContadorEnsayos); ContadorEnsayos++; if(TRADUCE==0) { display.Clear(); display.SetXY(0,0); display.printf("Comienza medida"); display.SetXY(0,1); display.printf("Viaje %d ...", ContadorEnsayos); } else { display.Clear(); display.SetXY(0,0); display.printf("Measuring ride"); display.SetXY(0,1); display.printf("number %d ...", ContadorEnsayos); } if(ContadorEnsayos == 100) { ContadorEnsayos = 0; } //EL NUMERO MAXIMO DE ENSAYOS ES 100. SI SE SUPERA SE PONE EL CONTADOR A 0 pc.printf("Ensayo: %d\n",ContadorEnsayos); fclose(fp2); fp2 = fopen("/mbedFlash/OrdEns.txt","w"); if(fp2 == NULL) { pc.printf("Could not open file OrdEns.txt for write\n"); } fprintf(fp2,"%d", ContadorEnsayos); fclose(fp2); ContadorDatos = 0; ContadorMedidas = 0; //Crea y abre un nuevo archivo para guardar aceleraciones char resultsFile[50]; strcpy(resultsFile,"/local/raw_V_"); int n = sprintf (NoViaje, "%d", ContadorEnsayos); strcat(resultsFile,NoViaje); strcat(resultsFile,".txt"); msc.disk_initialize(); fp = fopen(resultsFile,"w"); if(fp == NULL) { if(TRADUCE==0) { display.Clear();display.SetXY(0,0); display.printf("USB desconectado");display.SetXY(0,1); display.printf("o lleno"); } else { display.Clear();display.SetXY(0,0); display.printf("USB disconnected");display.SetXY(0,1); display.printf("or full"); } /////////////////////////////////////////////////////////////////////////////////////////// /// LO DE ABAJO ES PARA QUE NO SE QUEDE COLGADO SI HEMOS OLVIDADO CONECTAR EL PEN-DRIVE /// /////////////////////////////////////////////////////////////////////////////////////////// ensayo_red = false; ContadorEnsayos--; fp2 = fopen("/mbedFlash/OrdEns.txt","w"); if(fp2 == NULL) { pc.printf("Could not open file OrdEns.txt for write\n"); } fprintf(fp2,"%d", ContadorEnsayos); fclose(fp2); } ///////////////////////////////////////////////////////////////////// /// OJO: COMIENZA EL TICKER DE LECTURA DE DATOS DEL ACELERÓMETRO /// ///////////////////////////////////////////////////////////////////// if(ensayo_red == true) { tickerLectura.attach_us(&LeeAcelerometroBuffer, PasoTiempo); } } wait(0.5); } else //antes de pulsar estaba encendido { ensayo_red = false; // lo apaga, es decir acaba medidas y procesa datos si en modo autonomo if(ensayo_red == true) { red_LED = 1;} else { red_LED = 0;}//enciende o apaga el LED rojo pc.printf("-25717505\n"); //Señal para vmRideanalizer ///////////////////////////// /// APAGA EL ACELEROMETRO /// ///////////////////////////// bma180.AcelerometroSleep(); if(datosPC_green == false)//procesa datos si en modo autonomo. Cuando entra aquí, estan LEDs apagados + ensayo_red = false + datosPC_green == false { /////////////////////////////////////////// // POSTPROCESO DE DATOS EN MODO AUTONOMO //Primera tanda /////////////////////////////////////////// ///////////////////////////////////////////////////////////////////// /// OJO: FINALIZA EL TICKER DE LECTURA DE DATOS DEL ACELERÓMETRO /// ///////////////////////////////////////////////////////////////////// tickerLectura.detach(); fclose(fp);//cierra fichero con datos en el USB //if(TRADUCE==0) //{ // display.Clear(); // display.SetXY(0,0); display.printf("Interpola datos"); //}else //{ // display.Clear(); // display.SetXY(0,0); display.printf("Interpolating data"); // display.SetXY(0,1); display.printf("data"); //} //InterpolaMedidasPerdidas(ContadorEnsayos, ContadorDatos); if(TRADUCE==0) { display.Clear(); display.SetXY(0,0); display.printf("Rojo: posprocesa"); display.SetXY(0,1); display.printf("Amarillo: fin"); }else { display.Clear(); display.SetXY(0,0); display.printf("Red: postprocess"); display.SetXY(0,1); display.printf("Yellow: finalize"); } wait(0.5); while(1) { /////////////////////////////////////////// // POSTPROCESO DE DATOS EN MODO AUTONOMO //Primera tanda /////////////////////////////////////////// //loop infinito hasta que pulsa el rojo otra vez para ver mas resultados o el verde para terminar y grabar resultados en fichero if(red_PUSH.read() == 1) //si pulsa el rojo { //ponemos variables globales a zero //aceleraciones medias aZm0 = 0.0; aYm0 = 0.0; aXm0 = 0.0; //veloc truco Dani vZmFin = 0.0; vNom = 0.0; //maximos ISO amax = 0.0; dmax = 0.0; Vmax = 0.0; jerkmax = 0.0; max_peaktopeak_car_ = 0.0; max_peaktopeak_ncar_ = 0.0; max_peaktopeak_X_ = 0.0; max_peaktopeak_Y_ = 0.0; Vmax1 = 0.0; Vmax2 = 0.0; if(TRADUCE==0) { display.Clear(); display.SetXY(0,0); display.printf("Comienza el"); display.SetXY(0,1); display.printf("postproceso ..."); } else { display.Clear(); display.SetXY(0,0); display.printf("Starting"); display.SetXY(0,1); display.printf("postprocess ..."); } //InterpolaMedidasPerdidas(ContadorEnsayos, ContadorDatos); FiltradoPeriodosIntegracion(ContadorEnsayos, ContadorDatos); //display.Clear(); display.SetXY(0,0); display.printf("Interpolacion,"); display.SetXY(0,1); display.printf("filtro, integracion"); DatosConfort(ContadorEnsayos, ContadorDatos, PasoTiempo); //display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("Vel. nom. y jerk"); calcula_n05n95_Boveloc_jerkmax(ContadorEnsayos, ContadorDatos, PasoTiempo); //ha terminado de calcular amax, dmax, Vmax y jerkmax //display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("A95s"); calcula_A95_V_acel_decel(ContadorEnsayos, ContadorDatos, PasoTiempo); //ha terminado de calcular A95 de a, d y V. Muestra por pantalla DisplayResultados1(); if(FrecAdq == 320) { if(TRADUCE==0) { display.Clear(); display.SetXY(0,0); display.printf("Rojo continua"); display.SetXY(0,1); display.printf("Amarillo fin"); }else { display.Clear(); display.SetXY(0,0); display.printf("Red continue"); display.SetXY(0,1); display.printf("Yellow finalize"); } } break;//ha calculado resultados restantes y finaliza }else if(green_PUSH.read() == 1) { break; } }//primera tanda resultados while(FrecAdq == 320) { /////////////////////////////////////////// // POSTPROCESO DE DATOS EN MODO AUTONOMO //Segunda tanda /////////////////////////////////////////// //loop infinito hasta que pulsa el rojo otra vez para ver mas resultados o el verde para terminar y grabar resultados en fichero if(red_PUSH.read() == 1) //si pulsa el rojo { if(TRADUCE==0) { display.Clear(); display.SetXY(0,0); display.printf("Cont. Posproceso"); display.SetXY(0,1); display.printf("vibracion Z,X,Y"); }else { display.Clear(); display.SetXY(0,0); display.printf("Con. Postprocess"); display.SetXY(0,1); display.printf("vibration Z,X,Y"); } //empieza cálculos de ZXYvibweighted vibraReadFilter(ContadorEnsayos, ContadorDatos); //display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("step2"); calculaStep2( ContadorEnsayos, ContadorDatos); //display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("car-ncar"); calculaCarNCar( ContadorEnsayos, ContadorDatos, PasoTiempo ); //pc.printf("Termina 'calculaCarNCar'\n"); //display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("maxZXY"); calculaZXYvibweighted( ContadorEnsayos, ContadorDatos ); //display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("Display"); DisplayResultados2(); break;//ha calculado resultados restantes y finaliza }else if(green_PUSH.read() == 1) { break; } } //segunda tanda resultados //copia resultados en un fichero y finaliza if(TRADUCE==0) { display.Clear(); display.SetXY(0,0); display.printf("Guarda resultados"); display.SetXY(0,1); display.printf("en fichero"); }else { display.Clear(); display.SetXY(0,0); display.printf("Save results"); display.SetXY(0,1); display.printf("in file"); } FileResultados( ContadorEnsayos ); wait(0.5); if(TRADUCE==0) { display.Clear(); display.SetXY(0,0); display.printf("Postproceso"); display.SetXY(0,1); display.printf("fin"); }else { display.Clear(); display.SetXY(0,0); display.printf("Postprocess"); display.SetXY(0,1); display.printf("end"); } } wait(0.5); display.Clear(); } } if(ensayo_red == true) { red_LED = 1;} else { red_LED = 0;}//enciende o apaga el LED rojo ///////////////////////////// ///MODOS DE FUNCIONAMIENTO:// // AUTONOMO O PC //////////// ///////////////////////////// if(ensayo_red == false) { if(green_PUSH.read() == 1) { datosPC_green = !datosPC_green; if(datosPC_green == true) { if(TRADUCE == 0) { display.Clear(); display.SetXY(0,0); display.printf("Aparato en"); display.SetXY(0,1); display.printf("modo PC."); wait(1.5); display.Clear(); } else { display.Clear(); display.SetXY(0,0); display.printf("Set in PC mode."); wait(1.5); display.Clear(); } } if(datosPC_green == false) { if(TRADUCE == 0) { display.Clear(); display.SetXY(0,0); display.printf("Aparato en"); display.SetXY(0,1); display.printf("modo autonomo."); wait(1.5); display.Clear(); } else { display.Clear(); display.SetXY(0,0); display.printf("Set in"); display.SetXY(0,1); display.printf("autonomous mode."); wait(1.5); display.Clear(); } } } if(datosPC_green == true) { green_LED = 1; } else { green_LED = 0; }//enciende o apaga LED verde } ///////////////////////////// // MEDIDA DE ACELERACIONES // ///////////////////////////// if(ensayo_red == true) { if(datosPC_green == true)//Manda los datos al PC, puerto serie { //////////////////////////////////////////////// /// LECTURA CON ACELEROMETRO BMA180, DIGITAL /// //////////////////////////////////////////////// bma180.ReadAccels_BMA180(readings); int16_t aaX = (int16_t)readings[0]; int16_t aaY = (int16_t)readings[1]; int16_t aaZ = (int16_t)readings[2]; aX = (-1.0)*((float)aaX-(-103.23))*0.0018; aY = (-1.0)*((float)aaY-(57.21))*0.0018; aZ = (-1.0)*((float)aaZ-(701.1184))*0.0018097570403; ContadorMedidas++; pc.printf("%d,%f,%f,%f\n", timer.read_us(), aX, aY, aZ); } if(datosPC_green == false)//Manda los datos a fichero USB y promedia medidas perdidas { GuardaDatosBuffer(); /* //////////////////////////////////// // PROMEDIO MEDIDAS ACELERACIONES // //////////////////////////////////// if(ContadorMedidas == 1) {TiempoInicio = timer.read_us();} TiempoFin = timer.read_us(); if((TiempoFin-TiempoInicio)<((ContadorDatos+1)*PasoTiempo)) { nDatosPorPaso++; aXm += aX; aYm += aY; aZm += aZ; } else { //Calculo del numero de pasos en blanco int PasosEnBlanco = ((TiempoFin-TiempoInicio)-(ContadorDatos+1)*PasoTiempo)/(PasoTiempo); if(PasosEnBlanco == 0) { fprintf(fp,"%d %f %f %f %d\n", ContadorDatos*PasoTiempo, aXm/(float)nDatosPorPaso, aYm/(float)nDatosPorPaso, aZm/(float)nDatosPorPaso, nDatosPorPaso); aXm = aX; aYm = aY; aZm = aZ; nDatosPorPaso = 1; ContadorDatos ++; } else { for(int i = 0; i<PasosEnBlanco; i++) { fprintf(fp,"%d %f %f %f %d\n", (ContadorDatos+i)*PasoTiempo, -20.0, -20.0, -20.0, 0); //fprintf(fp,"t = %d, PasosEnblanco = %d, FinalMedida = %d \n", (ContadorDatos+i)*PasoTiempo, PasosEnBlanco,TiempoFin-TiempoInicio); aXm = aX; aYm = aY; aZm = aZ; } nDatosPorPaso = 1; ContadorDatos = ContadorDatos + PasosEnBlanco; } }*/ } } } } void InterpolaMedidasPerdidas(unsigned int ContadorEnsayos, unsigned int ContadorDatos) { int t; float aXYZ[3]; float aXYZantes[3]; float aXYZdespues[3]; float aXYZinterpolada[3]; int tPasosPerdidos[300]; FILE * fp; FILE * fp2; char NoViajel[2]; char resultsFile[50]; char interpolatedFile[50]; strcpy(resultsFile,"/local/raw_V_"); strcpy(interpolatedFile,"/local/IV_"); sprintf (NoViajel, "%d", ContadorEnsayos); strcat(resultsFile,NoViajel); strcat(interpolatedFile,NoViajel); strcat(resultsFile,".txt"); strcat(interpolatedFile,".dat"); fp = fopen(resultsFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", resultsFile); } fp2 = fopen(interpolatedFile,"w"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", interpolatedFile); } unsigned int i = 0; unsigned int nDatosPorPaso; unsigned int PasosPerdidos = 0; while(i<ContadorDatos) { fscanf(fp,"%d %f %f %f %d", &t, &aXYZ[0], &aXYZ[1], &aXYZ[2], &nDatosPorPaso); if(nDatosPorPaso >0) { aXYZantes[0] = aXYZ[0]; aXYZantes[1] = aXYZ[1]; aXYZantes[2] = aXYZ[2]; fprintf(fp2,"%f %f %f %f\n", (float)t/1000000.0, aXYZ[0], aXYZ[1], aXYZ[2]); i++; } else { tPasosPerdidos[PasosPerdidos] = t; PasosPerdidos = 1; while(nDatosPorPaso == 0) { fscanf(fp,"%d %f %f %f %d", &t, &aXYZ[0], &aXYZ[1], &aXYZ[2], &nDatosPorPaso); if(nDatosPorPaso == 0) { tPasosPerdidos[PasosPerdidos] = t; PasosPerdidos++; } else { aXYZdespues[0] = aXYZ[0]; aXYZdespues[1] = aXYZ[1]; aXYZdespues[2] = aXYZ[2]; } } for(unsigned int j = 0; j<PasosPerdidos; j++) { //aXYZinterpolada[0] = aXYZantes[0]+((aXYZdespues[0]-aXYZantes[0])/(float(PasosPerdidos+1)))*(float)(j+1); //aXYZinterpolada[1] = aXYZantes[1]+((aXYZdespues[1]-aXYZantes[1])/(float(PasosPerdidos+1)))*(float)(j+1); //aXYZinterpolada[2] = aXYZantes[2]+((aXYZdespues[2]-aXYZantes[2])/(float(PasosPerdidos+1)))*(float)(j+1); aXYZinterpolada[0] = (aXYZdespues[0]+aXYZantes[0])/2.0; aXYZinterpolada[1] = (aXYZdespues[1]+aXYZantes[1])/2.0; aXYZinterpolada[2] = (aXYZdespues[2]+aXYZantes[2])/2.0; fprintf(fp2,"%f %f %f %f\n", (float)tPasosPerdidos[j]/1000000.0, aXYZinterpolada[0], aXYZinterpolada[1], aXYZinterpolada[2]); i++; } fprintf(fp2,"%f %f %f %f\n", (float)t/1000000.0, aXYZ[0], aXYZ[1], aXYZ[2]); i++; PasosPerdidos = 0; } } fclose(fp); fclose(fp2); //elimina fichero RAW int rm = remove(resultsFile); } void FiltradoPeriodosIntegracion(unsigned int ContadorEnsayos, unsigned int ContadorDatos) { float t; float aXYZ[3]; //vector aceleracion en el paso i [aX,aY,aZ] float xv[3]; //Para hacer las operaciones del Filtro Butterworth float yv[3]; //Para hacer las operaciones del Filtro Butterworth const float tol = 0.05; //Porcentaje en que la aceleracion debe superar la media para considerar que cambia el periodo del viaje bool mov = false; //Indica si hay o no movimiento bool velV = false; //Indica si estamos en periodo de velocidad constante bool decel = false; //Indica si ha empezado la deceleracion bool fin = false; //Indica si ha terminado el viaje //float t_Arr, t_V0, t_Vfin, t_Par; //Instantes en que empiezan arranque, ... float xZ = 0.0; //Posici�n vertical calculada integrando vZ float vZ = 0.0; //Velocidad vertical calculada integrando la aceleraciOn filtrada menos su media float t_3[3], aZ_3[3], vZ_3[3], xZ_3[3]; //Datos guardados para hacer integracion numerica de aceleraciones float xa, xb, xc, h, fa, fb, fc, fab, fbc, fabc; unsigned int nmedV = 50; //Puntos para calcular la media de vZ al final del viaje (que debe ser zero pero no lo es) float sgnAcelArr = 0.0; //Signo de la aceleracion vertical de arranque float sgnAcelFr = 0.0; //Signo de la aceleracion vertical de frenada float sgnAcelv0 = 0.0; //Signo de la aceleracion vertical al comienzo del periodo de vel cte float sgnAcelFin = 0.0; //Signo de la aceleracion vertical al final del periodo de vel cte const float tiempoMinimoArranque = 0.15; //Tiempo mínimo que puede durar la fase de aceleracion de la cabina const float tiempoMinimoVelCte = 0.15; //Tiempo mínimo que puede durar el periodo de velocidad constante const float tiempoMinimoFrenada = 0.15; //Tiempo mínimo que puede durar el periodo de deceleracion de la cabina 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;} FILE * fp; FILE * fp3; char NoViajel[2]; char resultsFile[50]; char dispVelFile[50]; int dif; //strcpy(resultsFile,"/local/IV_"); strcpy(dispVelFile,"/local/dva0_"); strcpy(resultsFile,"/local/raw_V_"); strcpy(dispVelFile,"/local/dva0_"); sprintf (NoViajel, "%d", ContadorEnsayos); strcat(resultsFile,NoViajel); strcat(dispVelFile,NoViajel); strcat(resultsFile,".txt"); strcat(dispVelFile,".txt"); //fichero con todos los datos de la funcion anterior IV_ 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); } //fichero donde guardaron 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); } //Lee, filtra y guarda las tres aceleraciones por separado for(unsigned int i = 0; i<ContadorDatos; i++) { 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()) /////////////////////////////////////////////////////// /// FILTRO DE BUTTERWORTH, PASO BAJO, ORDEN 2, 10Hz /// /////////////////////////////////////////////////////// if(i < 2) { xv[i+1] = aXYZ[2]; yv[i+1] = aXYZ[2]; } else { xv[0] = xv[1]; xv[1] = xv[2]; xv[2] = aXYZ[2] / gain; yv[0] = yv[1]; yv[1] = yv[2]; yv[2] = (xv[0] + xv[2]) + 2.0 * xv[1] + ( c_butt1 * yv[0]) + ( c_butt2 * yv[1]); aXYZ[2] = yv[2]; } ///////////////////////////////////////////////////////////////////////// /// DETECCION INSTANTES DE ARRANQUE, COMIENZO Vcte, FIN Vcte Y PARADA /// ///////////////////////////////////////////////////////////////////////// /// CALCULO DE ACELERACION Z DE REFERENCIA (DEBE COINCIDIR CON LA GRAVEDAD: 9.81) aZm0 if( (i >= ndes)&&(i < (ndes+nmed))) { aXm0 += aXYZ[0]/(float)nmed; aYm0 += aXYZ[1]/(float)nmed; aZm0 += aXYZ[2]/(float)nmed; } //Calculo nArr,nV0,nVfin,nPar if( (i >= (ndes+nmed))) { if ((mov == false) && (fin == false)) { if(fabs(aXYZ[2]-aZm0) > fabs(tol*aZm0)) { nArr = i-(ndes+nmed); //Detecta el instante de arranque mov = true; t_Arr = t; sgnAcelArr = (aXYZ[2]-aZm0)/fabs(aXYZ[2]-aZm0); //Calcula el signo de la aceleracion vertical de arranque //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); } } if ( (mov == true) && (velV == false) && (decel == false) && (t-t_Arr>tiempoMinimoArranque) ) { if(fabs(aXYZ[2]-aZm0) < fabs(tol*aZm0)) { sgnAcelv0 = (aXYZ[2]-aZm0)/fabs(aXYZ[2]-aZm0); if(fabs(sgnAcelv0-sgnAcelArr)>1.5) //El signo de la aceleracion de arranque tiene que ser distinto del signo de la de frenada { nV0 = i-(ndes+nmed); //Detecta el instante de inicio del periodo de V cte velV = true; t_V0 = t; //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); } } } if ( (mov == true) && (velV == true) && (decel == false) && (t-t_V0>tiempoMinimoVelCte)) { if(fabs(aXYZ[2]-aZm0) > fabs(tol*aZm0)) { sgnAcelFr = (aXYZ[2]-aZm0)/fabs(aXYZ[2]-aZm0); if(fabs(sgnAcelFr-sgnAcelArr)>1.5) //El signo de la aceleracion de arranque tiene que ser distinto del signo de la de frenada { nVfin = i-(ndes+nmed); //Detecta el instante de fin del periodo de V cte velV = false; decel = true; t_Vfin = t; //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); } } } if ( (mov == true) && (velV == false) && (decel == true) && (t-t_Vfin>tiempoMinimoFrenada)) { if(fabs(aXYZ[2]-aZm0) < fabs(tol*aZm0)) { sgnAcelFin = (aXYZ[2]-aZm0)/fabs(aXYZ[2]-aZm0); if(fabs(sgnAcelFin-sgnAcelFr)>1.5) { nPar = i-(ndes+nmed); //Detecta el instante de parada velV = false; decel = false; mov = false; fin = true; t_Par = t; //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); } } } } //////////////////////////////////////////////////////////////////////////// /// CALCULO DE POSICION Y VELOCIDAD INTEGRANDO LA ACELERACION Z FILTRADA /// //////////////////////////////////////////////////////////////////////////// int i2 = i-(ndes+nmed); //Nuevo idice para calcular posicion y velocidad //if (i2 == 0) {t_int = t;} //Ojo con los primeros valores //actualizo valores t_3, aZ_3 t_3[0] = t_3[1]; t_3[1] = t_3[2]; t_3[2] = t; aZ_3[0] = aZ_3[1]; aZ_3[1] = aZ_3[2]; aZ_3[2] = aXYZ[2]-aZm0; if(i2 >= 0) { vZ_3[0] = vZ_3[1]; vZ_3[1] = vZ_3[2]; xZ_3[0] = xZ_3[1]; xZ_3[1] = xZ_3[2]; xa = t_3[2]; xb = t_3[1]; xc = t_3[0]; h = (xa-xb)/2.0; fa = aZ_3[2]; fb = aZ_3[1]; fc = aZ_3[0]; //if(fabs(xa-xb)>1.0e-9) {fab = (fa-fb)/(xa-xb);} else {fab = 0.0;} //if(fabs(xb-xc)>1.0e-9) {fbc = (fb-fc)/(xb-xc);} else {fbc = 0.0;} //if(fabs(xa-xc)>1.0e-9) {fabc = (fab-fbc)/(xa-xc);} else {fabc = 0.0;} fab = (fa-fb)/(xa-xb); fbc = (fb-fc)/(xb-xc); fabc = (fab-fbc)/(xa-xc); vZ_3[2]= vZ_3[1] + fa*2.0*h + fab*(-2.0*h*h) + fabc*(-4.0*h*h*h/6.0); // Posicion fa = vZ_3[2]; fb = vZ_3[1]; fc = vZ_3[0]; //if(fabs(xa-xb)>1.0e-9) {fab = (fa-fb)/(xa-xb);} else {fab = 0.0;} //if(fabs(xb-xc)>1.0e-9) {fbc = (fb-fc)/(xb-xc);} else {fbc = 0.0;} //if(fabs(xa-xc)>1.0e-9) {fabc = (fab-fbc)/(xa-xc);} else {fabc = 0.0;} fab = (fa-fb)/(xa-xb); fbc = (fb-fc)/(xb-xc); fabc = (fab-fbc)/(xa-xc); xZ_3[2] = xZ_3[1] + fa*2.0*h + fab*(-2.0*h*h) + fabc*(-4.0*h*h*h/6.0); 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 } //CALCULO DE LOS DESPLAZAMIENTOS EN INSTANTES DE ARRANQUE Y PARADA if(i-(ndes+nmed) == nArr) {d_arr = xZ_3[2];} if(i-(ndes+nmed) == nPar) {d_par = xZ_3[2];} //CALCULO VELOCIDAD MEDIA EN INSTANTES FINALES //PARA IMPLEMENTAR EL TRUCO DE DANI if((ContadorDatos-i) <= nmedV) { vZmFin += vZ_3[2]/(float)nmedV; tFin = t_3[2]; } //Calculo de las velociades obtenidas por integración (que deben ser 0 en realidad) en los instantes de arranque y parada if(nArr == (i-(ndes+nmed))) {vArr = vZ_3[2];} if(nPar == (i-(ndes+nmed))) {vPar = vZ_3[2];} }//Fin loop principal fclose(fp); fclose(fp3); } void DatosConfort(unsigned int ContadorEnsayos, unsigned int ContadorDatos, int PasoTiempo) { float t; float dZ, vZ, aZ; const float tolV = 0.01; //Tolerancia, en tanto por uno de la velocidad nominal, para recalcular el tiempo final del viaje unsigned int RecalculaNPar = 0; //Se pone a 1 para recalcular el instante de parada unsigned int CalculaBo1 = 0; //Se pone a 1 para calcular el limite Bo1 unsigned int CalculaBo2 = 0; //Se pone a 1 para calcular el limite Bo2 Bo0 = 0; //Se establece el Bo0 al principio, tras los puntos descartados y los de calculo de g Bo3 = ContadorDatos-(ndes+nmed)-1; //Se establece el Bo3 al final, contando los puntos descartados y los de calculo de g if(Bo3>4000000000){Bo3 = 0;} Boacel = (unsigned int)(nV0 + nVfin)*0.5; //Se establece inicialmente (luego se vera si tenemos que cambiarlo) como la media entre nV0 y nV //float amax = 0.0;//VALOR POR PANTALLA //float dmax = 0.0;//VALOR POR PANTALLA //float Vmax = 0.0;//VALOR POR PANTALLA //Relativos al calculo del jerk const float running_time = 0.5; int Ai = (int)(0.5*running_time*1.0e6)/PasoTiempo; float aZ_05s[2*Ai+1]; //Aceleraciones dentro del periodo de 0.5 segundos para hacer regresion lineal float jerk_025s[Ai+1]; //Jerks dentro del periodo de 0.25 for (unsigned int i = 0; i<(2*Ai+1); i++) { aZ_05s[i] = 0.0; } for (unsigned int i = 0; i<(Ai+1); i++) { jerk_025s[i] = 0.0; } ////////////////////////////////////////////////////////////////////////////// // Datos para nueva integración de la velocidad para obtener desplazamiento // ////////////////////////////////////////////////////////////////////////////// float t_3[3], vZ_3[3], xZ_3[3]; //Datos guardados para hacer integracion numerica de aceleraciones float xa, xb, xc, h, fa, fb, fc, fab, fbc, fabc; for (unsigned int i = 0; i<3; i++) { t_3[i] = 0.0; vZ_3[i] = 0.0; xZ_3[i] = 0.0; } //nuevo Juan, soluciona error en nuevo calculo de dZ t_3[2] = (ndes + nmed - 1)*PasoTiempo/1000000.0; t_3[1] = (ndes + nmed - 2)*PasoTiempo/1000000.0; //Archivos de lectura y escritura FILE * fp; FILE * fp2; char NoViajel[2]; char dispVelFile[50]; char velMod[50]; strcpy(dispVelFile,"/local/dva0_"); strcpy(velMod,"/local/dvaj_"); sprintf (NoViajel, "%d", ContadorEnsayos); strcat(dispVelFile,NoViajel); strcat(velMod,NoViajel); strcat(dispVelFile,".txt"); strcat(velMod,".txt"); fp = fopen(dispVelFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", dispVelFile); } fp2 = fopen(velMod,"w"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", velMod); } //Lee velocidad para establecer periodos y recalcular nPar. Tambnien se calculan Bo1 y Bo2 int nDatos = (ContadorDatos-(ndes+nmed)); if (nDatos < 0) {nDatos = 0;} float t_in; 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 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 for(int i = 0; i < nDatos; i++) { fscanf(fp,"%f %f %f %f", &t, &dZ, &vZ, &aZ);//aZ filtered, aX y aY raw. if(i == 0) { t_in = t;} //vZ = vZ - vZmFin*(t/tFin); //CAMBIO JL (11/10/2013) CON ESTA FORMA LA VELOCIDAD NO PARTE DE 0 //vZ = vZ - vZmFin*((t-t_in)/(tFin-t_in)); //Truco de Dani mas sofisticado if(i < nArr) { vZ = vZ - vArr*((t-t_in)/(t_Arr-t_in));} if(i >= nArr) { vZ = vZ - (a_LV*t + b_LV);} // NUEVA INTEGRACIÓN DE LA VELOCIDAD PARA CALCULAR DESPLAZAMIENTO t_3[0] = t_3[1]; t_3[1] = t_3[2]; t_3[2] = t; vZ_3[0] = vZ_3[1]; vZ_3[1] = vZ_3[2]; vZ_3[2] = vZ; xZ_3[0] = xZ_3[1]; xZ_3[1] = xZ_3[2]; xa = t_3[2]; xb = t_3[1]; xc = t_3[0]; h = (xa-xb)/2.0; fa = vZ_3[2]; fb = vZ_3[1]; fc = vZ_3[0]; //if(fabs(xa-xb)>1.0e-9) {fab = (fa-fb)/(xa-xb);} else {fab = 0.0;} //if(fabs(xb-xc)>1.0e-9) {fbc = (fb-fc)/(xb-xc);} else {fbc = 0.0;} //if(fabs(xa-xc)>1.0e-9) {fabc = (fab-fbc)/(xa-xc);} else {fabc = 0.0;} fab = (fa-fb)/(xa-xb); fbc = (fb-fc)/(xb-xc); fabc = (fab-fbc)/(xa-xc); xZ_3[2] = xZ_3[1] + fa*2.0*h + fab*(-2.0*h*h) + fabc*(-4.0*h*h*h/6.0); dZ = xZ_3[2]; if(i == nArr) {d_arr = dZ;} if(i == nPar) {d_par = dZ;} // HASTA AQUÍ if( (i>=nV0)&&(i<nVfin) ) { vNom += fabs(vZ)/(float)(nVfin-nV0); } if( (i == nPar) && (fabs(vZ) > tolV*fabs(vNom)) ) { RecalculaNPar = 1; } if( (RecalculaNPar == 1) && (fabs(vZ) < tolV*fabs(vNom)) ) { nPar = i; t_Par = t; RecalculaNPar = 0; } // Calculo de Bo1 if( i == nArr) { CalculaBo1 = 1; } if ((fabs(fabs(dZ)-fabs(d_arr))>0.5) && (CalculaBo1 == 1)) { Bo1 = i; CalculaBo1 = 0; } //OJO: INTENTAR CORREGIR Bo1 CON NUEVO PUNTO DE PARADA arranque // Calculo de Bo2 //OJO: INTENTAR CORREGIR Bo2 CON NUEVO PUNTO DE PARADA nPar if( i == nV0) { CalculaBo2 = 1; } if ((fabs(fabs(dZ)-fabs(d_par))<0.5) && (CalculaBo2 == 1) && (i>Bo1)) { Bo2 = i; CalculaBo2 = 0; } //Calculo maxima aceleracion y deceleracion. Calcula Vmax1 y Vmax2. n05Vmaxi y n95Vmaxi en la siguiente pasada. if ( (i>=Bo0) && (i<Boacel)) { if(fabs(aZ)>amax) { amax = fabs(aZ);} if(fabs(vZ)>Vmax1) { Vmax1 = fabs(vZ);} } if ( (i>=Boacel) && (i<=Bo3)) { if(fabs(aZ)>dmax) { dmax = fabs(aZ);} if(fabs(vZ)>Vmax2) { Vmax2 = fabs(vZ);} } //Calculo de jerk por regresion for(unsigned int j = 0; j<(2*Ai+1); j++) { aZ_05s[j] = aZ_05s[j+1]; } aZ_05s[2*Ai] = aZ; for(unsigned int j = 0; j<(Ai+1); j++) { jerk_025s[j] = jerk_025s[j+1]; } jerk_025s[Ai] = RegresionJerk(aZ_05s, 2*Ai+1); //Ojo, el jerk esta desfasado 0.25 segundos. Claculo su m�xdimo en la siguiente pasada. //calculo Vmax a partir de Vmax1 y Vmax2. if (Vmax1<Vmax2) { Vmax = Vmax2; }else { Vmax = Vmax1; } if(i== nPar) {d_par = dZ;} fprintf(fp2,"%f %f %f %f %f \n", t, dZ, vZ, aZ, jerk_025s[0]); } fclose(fp); fclose(fp2); //ELIMINA ARCHIVO dva0 int rmDVA0 = remove(dispVelFile); if(rmDVA0 != 0) { display.Clear();display.SetXY(0,0); display.printf("Error removing %s file\n", dispVelFile); } } void calcula_n05n95_Boveloc_jerkmax(unsigned int ContadorEnsayos, unsigned int ContadorDatos, int PasoTiempo) { //float jerkmax = 0.0;//POR PANTALLA float t; float dZ, vZ, aZ, jerk; int CalculaBo2(0); //5% and 95% of Vmax1 y Vmax2 unsigned int c1(0);unsigned int c2(0);unsigned int c3(0);unsigned int c4(0); //Velocidad A95 const float A95max_veloc_acel = 0.95*Vmax1; const float A95max_veloc_decel = 0.95*Vmax2; unsigned int contA95 = Bo1; unsigned int contA95_2 = Boacel + 1; bool Bo_veloc_acel_done = false; bool Bo_veloc_decel_done = false; float t2(0.0); float t3(0.0); //Archivos de lectura y escritura FILE * fp; char NoViajel[2]; char velMod[50]; strcpy(velMod,"/local/dvaj_"); sprintf (NoViajel, "%d", ContadorEnsayos); strcat(velMod,NoViajel); strcat(velMod,".txt"); fp = fopen(velMod,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", velMod); } int nDatos = (ContadorDatos-(ndes+nmed)); if (nDatos < 0) {nDatos = 0;} //loop principal for(unsigned int i = 0; i<nDatos; i++) { //read data row fscanf(fp,"%f %f %f %f %f", &t, &dZ, &vZ, &aZ, &jerk); if( i == nV0) { CalculaBo2 = 1; } if ( (fabs(fabs(dZ)-fabs(d_par))<0.5) && (i>Bo1) && (CalculaBo2 == 1) ) { Bo2 = i; CalculaBo2 = 0; } //jerkmax if ( fabs(jerk) >= jerkmax ) { jerkmax = fabs(jerk); } //5% and 95% of Vmax1 if ( (i >= Bo0) && (i <= Boacel) ) { if( ( fabs(vZ) > 0.05*Vmax1 ) && (c1 == 0) ) { n05Vmax1 = i; c1 = 1; } if( ( fabs(vZ) > 0.95*Vmax1 ) && (c2 == 0) ) { n95Vmax1 = i; c2 = 1; } } //5% and 95% of Vmax2 if ( (i >= (Boacel + 1)) && (i < Bo3) ) { if( ( fabs(vZ) < 0.95*Vmax2 ) && (c3 == 0) ) { n95Vmax2 = i; c3 = 1; } if( ( fabs(vZ) < 0.05*Vmax2 ) && (c4 == 0) ) { n05Vmax2 = i; c4 = 1; } } //Bo_veloc_acel y Bo_veloc_decel if( ( (i > Bo1) && (i<=Boacel) ) && ( Bo_veloc_acel_done == false ) ) { if ( fabs(vZ) >= A95max_veloc_acel ) { Bo_veloc_acel = contA95; t2 = t + 1.0; Bo_veloc_acel_done = true; } contA95++; } if( ( (i > (Boacel + 1)) && (i<=Bo3)) && (Bo_veloc_decel_done == false) ) { if ( fabs(vZ) >= A95max_veloc_decel ) { Bo_veloc_decel = contA95_2; t3 = t - 1.0; Bo_veloc_decel_done = true; } contA95_2++; } }//loop principal //calculos after loop //t2 y t3 for (unsigned int j = 0 ; j<=100 ; j++) { if(t3<t2) { t2 = t2 - ((float)(1))/10.0; t3 = t3 + ((float)(1))/10.0; }else { Bo_veloc_acel = (unsigned int)( Bo_veloc_acel + (1.0/((float)(PasoTiempo/1000000.0))) - ((float)(j))/(10.0*((float)(PasoTiempo/1000000.0))) ); Bo_veloc_decel = (unsigned int)( Bo_veloc_decel - (1.0/((float)(PasoTiempo/1000000.0))) + ((float)(j))/(10.0*((float)(PasoTiempo/1000000.0))) ); break; } } //seguridad //seguridad por si nVmax1 if ( c1 == 0 ) { n05Vmax1 = (unsigned int)(Bo0 + (Bo1-Bo0)/2.0); } if ( c2 == 0 ) { n95Vmax1 = Boacel; } //seguridad por si nVmax2 if ( c3 == 0 ) { n95Vmax2 = Boacel; } if ( c4 == 0 ) { n05Vmax2 = (unsigned int)(Bo2 + (Bo3-Bo2)/2.0); } //cierra ficheros fclose(fp); } void calcula_A95_V_acel_decel(unsigned int ContadorEnsayos, unsigned int ContadorDatos, int PasoTiempo) { float t; float dZ, vZ, aZ, jerk; //VA95 int length_maximoVA95(0); if( Bo_veloc_decel >= Bo_veloc_acel ) { length_maximoVA95 = (int)((Bo_veloc_decel - Bo_veloc_acel)*0.05) + 1; }else { length_maximoVA95 = 1; } //int length_maximoVA95 = (int)((Bo_veloc_decel - Bo_veloc_acel)*0.05) + 1; int maxindex(0); vector<float> *maximoAV95 = new vector<float> (length_maximoVA95,0.0); //vector<float> maximoAV95 (length_maximoVA95,0.0); vector<float> *diferencia_vec = new vector<float>; vector<float>::iterator up; float diferencia; float maximo; int contadorA95(0); //aA95 int length_maximo_aA95(0); if( n95Vmax1 >= n05Vmax1 ) { length_maximo_aA95 = (int)((n95Vmax1 - n05Vmax1)*0.05) + 1; }else { length_maximo_aA95 = 1; } //int length_maximo_aA95 = (int)((n95Vmax1 - n05Vmax1)*0.05) + 1; int maxindex_aA95(0); vector<float> *maximo_aA95 = new vector<float> (length_maximo_aA95,0.0); //vector<float> maximo_aA95 (length_maximo_aA95,0.0); vector<float> *diferencia_aA95_vec = new vector<float>; vector<float>::iterator up_aA95; float diferencia_aA95; float maximoaA95; int contador_aA95(0); //dA95 int length_maximo_dA95(0); if( n05Vmax2 >= n95Vmax2 ) { length_maximo_dA95 = (int)((n05Vmax2 - n95Vmax2)*0.05) + 1; }else { length_maximo_dA95 = 1; } //int length_maximo_dA95 = (int)((n05Vmax2 - n95Vmax2)*0.05) + 1; int maxindex_dA95(0); vector<float> *maximo_dA95 = new vector<float> (length_maximo_dA95,0.0); //vector<float> maximo_dA95 (length_maximo_dA95,0.0); vector<float> *diferencia_dA95_vec = new vector<float>; vector<float>::iterator up_dA95; float diferencia_dA95; float maximodA95; int contador_dA95(0); //Archivos de lectura y escritura FILE * fp; char NoViajel[2]; char data[50]; strcpy(data,"/local/dvaj_"); sprintf (NoViajel, "%d", ContadorEnsayos); strcat(data,NoViajel); strcat(data,".txt"); fp = fopen(data,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", data); } int nDatos = (ContadorDatos-(ndes+nmed)); if (nDatos < 0) {nDatos = 0;} if ( (maximoAV95 == NULL) || ( maximo_aA95 == NULL) || ( maximo_dA95 == NULL) ) { display.Clear();display.SetXY(0,0); display.printf("RAM overflow\n"); }else { //loop principal, pasada 1 for(unsigned int i = 0; i<(nDatos); i++) { //read data row fscanf(fp,"%f %f %f %f %f", &t, &dZ, &vZ, &aZ, &jerk); //VA95 if( (i <= Bo_veloc_decel) && ( i >= Bo_veloc_acel) ) { //pasada 1 for (unsigned int j = 0; j<length_maximoVA95; j++) { diferencia = fabs(vZ) - maximoAV95->at(j); if ( diferencia < 0 ) { diferencia_vec->push_back(0.0); contadorA95++; }else { diferencia_vec->push_back(diferencia); } } if ( contadorA95 == length_maximoVA95 ){} else { maximo = *max_element(diferencia_vec->begin(),diferencia_vec->end()); up = find(diferencia_vec->begin(), diferencia_vec->end(), maximo); maxindex = int(up - diferencia_vec->begin()); maximoAV95->erase(maximoAV95->begin() + maxindex ); maximoAV95->insert(maximoAV95->begin() + maxindex ,fabs(vZ)); } contadorA95 = 0; diferencia_vec->clear(); } //aA95 if( (i <= n95Vmax1) && ( i >= n05Vmax1) ) { //pasada 1 for (unsigned int j = 0; j<length_maximo_aA95; j++) { diferencia_aA95 = fabs(aZ) - maximo_aA95->at(j); if ( diferencia_aA95 < 0 ) { diferencia_aA95_vec->push_back(0.0); contador_aA95++; }else { diferencia_aA95_vec->push_back(diferencia_aA95); } } if ( contador_aA95 == length_maximo_aA95 ){} else { maximoaA95 = *max_element(diferencia_aA95_vec->begin(),diferencia_aA95_vec->end()); up_aA95 = find(diferencia_aA95_vec->begin(), diferencia_aA95_vec->end(), maximoaA95); maxindex_aA95 = int(up_aA95 - diferencia_aA95_vec->begin()); maximo_aA95->erase(maximo_aA95->begin() + maxindex_aA95 ); maximo_aA95->insert(maximo_aA95->begin() + maxindex_aA95 ,fabs(aZ)); } contador_aA95 = 0; diferencia_aA95_vec->clear(); } //dA95 if( (i <= n05Vmax2) && ( i >= n95Vmax2) ) { //pasada 1 for (unsigned int j = 0; j<length_maximo_dA95; j++) { diferencia_dA95 = fabs(aZ) - maximo_dA95->at(j); if ( diferencia_dA95 < 0 ) { diferencia_dA95_vec->push_back(0.0); contador_dA95++; }else { diferencia_dA95_vec->push_back(diferencia_dA95); } } if ( contador_dA95 == length_maximo_dA95 ){} else { maximodA95 = *max_element(diferencia_dA95_vec->begin(),diferencia_dA95_vec->end()); up_dA95 = find(diferencia_dA95_vec->begin(), diferencia_dA95_vec->end(), maximodA95); maxindex_dA95 = int(up_dA95 - diferencia_dA95_vec->begin()); maximo_dA95->erase(maximo_dA95->begin() + maxindex_dA95 ); maximo_dA95->insert(maximo_dA95->begin() + maxindex_dA95 ,fabs(aZ)); } contador_dA95 = 0; diferencia_dA95_vec->clear(); } }//Fin loop principal, pasada 1. sort(maximoAV95->begin(),maximoAV95->end());//ascending reverse(maximoAV95->begin(),maximoAV95->end());//descending sort(maximo_aA95->begin(),maximo_aA95->end());//ascending reverse(maximo_aA95->begin(),maximo_aA95->end());//descending sort(maximo_dA95->begin(),maximo_dA95->end());//ascending reverse(maximo_dA95->begin(),maximo_dA95->end());//descending //cierra ficheros y variables fclose(fp); //pasada2 fp = fopen(data,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", data); } //loop principal, pasada 2 double resta(1000.0); double resta_aA95(1000.0); double resta_dA95(1000.0); for(unsigned int i = 0; i<(nDatos); i++) { //read data row fscanf(fp,"%f %f %f %f %f", &t, &dZ, &vZ, &aZ, &jerk); //VA95 if( (i <= Bo_veloc_decel) && ( i >= Bo_veloc_acel) ) { if( ((maximoAV95->at(maximoAV95->size() - 1) - fabs(vZ)) <= resta) && (fabs(vZ) < maximoAV95->at(maximoAV95->size() - 1)) ) { VA95 = fabs(vZ); resta = maximoAV95->at(maximoAV95->size() - 1) - fabs(vZ); } } //aA95 if( (i <= n95Vmax1) && ( i >= n05Vmax1) ) { if( ((maximo_aA95->at(maximo_aA95->size() - 1) - fabs(aZ)) <= resta_aA95) && (fabs(aZ) < maximo_aA95->at(maximo_aA95->size() - 1)) ) { aA95 = fabs(aZ); resta_aA95 = maximo_aA95->at(maximo_aA95->size() - 1) - fabs(aZ); } } //dA95 if( (i <= n05Vmax2) && ( i >= n95Vmax2) ) { if( ((maximo_dA95->at(maximo_dA95->size() - 1) - fabs(aZ)) <= resta_dA95) && (fabs(aZ) < maximo_dA95->at(maximo_dA95->size() - 1)) ) { dA95 = fabs(aZ); resta_dA95 = maximo_dA95->at(maximo_dA95->size() - 1) - fabs(aZ); } } } //clear all vectors maximoAV95->clear(); delete maximoAV95; diferencia_vec->clear(); delete diferencia_vec; maximo_aA95->clear(); delete maximo_aA95; diferencia_aA95_vec->clear(); delete diferencia_aA95_vec; maximo_dA95->clear(); delete maximo_dA95; diferencia_dA95_vec->clear(); delete diferencia_dA95_vec; }//fin else fclose(fp); } void vibraReadFilter(unsigned int ContadorEnsayos, unsigned int ContadorDatos) { float t; float aXYZ[3]; //vector aceleracion en el paso i [aX,aY,aZ] float Zvibweighted[3]={0.0,0.0,0.0}; float Xvibweighted[3]={0.0,0.0,0.0}; float Yvibweighted[3]={0.0,0.0,0.0}; float aZ[3], aX[3], aY[3]; FILE * fp; FILE * fp3; char NoViajel[2]; char resultsFile[50]; char resultsFileW[50]; //strcpy(resultsFile,"/local/IV_"); strcpy(resultsFile,"/local/raw_V_"); strcpy(resultsFileW,"/local/W_IV_"); sprintf (NoViajel, "%d", ContadorEnsayos); strcat(resultsFile,NoViajel); strcat(resultsFileW,NoViajel);//fichero con todos los datos de la función anterior IV_ strcat(resultsFile,".txt"); strcat(resultsFileW,".txt"); //fichero con todos los datos de la función anterior IV_ fp = fopen(resultsFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", resultsFile); } //fichero donde guardaré fp3 = fopen(resultsFileW,"w"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", resultsFileW); } //varias pasadas para isofiltrar ax ay y az. isofilter son 4 filtros, pues 4 pasadas //Hemos recordado //loop principal. Pasada 1. Hl (Low Pass 100Hz Orden 2) for(unsigned int i = 0; i<(ContadorDatos); i++) { fscanf(fp,"%f %f %f %f", &t, &aXYZ[0], &aXYZ[1], &aXYZ[2]);//lee tiempo y 3 aceleraciones X, Y, Z //restamos gravedad a aZ aXYZ[2] = aXYZ[2] - aZm0; aXYZ[0] = aXYZ[0] - aXm0; aXYZ[1] = aXYZ[1] - aYm0; //isofilter int i2 = i-(ndes+nmed); //Nuevo idice para calcular posicion y velocidad if ( i==0 ) { Zvibweighted[0] = aXYZ[2]; Xvibweighted[0] = aXYZ[0]; Yvibweighted[0] = aXYZ[1]; aZ[0] =aXYZ[2]; aX[0] =aXYZ[0]; aY[0] =aXYZ[1]; // fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[0], Yvibweighted[0], Zvibweighted[0]); }else if ( i==1 ) { Zvibweighted[1] = aXYZ[2]; Xvibweighted[1] = aXYZ[0]; Yvibweighted[1] = aXYZ[1]; aZ[1] =aXYZ[2]; aX[1] =aXYZ[0]; aY[1] =aXYZ[1]; // fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[1], Yvibweighted[1], Zvibweighted[1]); }else { aZ[2] = aXYZ[2]; aX[2] = aXYZ[0]; aY[2] = aXYZ[1]; Zvibweighted[2] = isoFilter_Wk_z_Hl(Zvibweighted[0],Zvibweighted[1],aZ, PasoTiempo); Xvibweighted[2] = isoFilter_Wd_xy_Hl(Xvibweighted[0],Xvibweighted[1],aX, PasoTiempo); Yvibweighted[2] = isoFilter_Wd_xy_Hl(Yvibweighted[0],Yvibweighted[1],aY, PasoTiempo); if(i2 >= 0) { fprintf(fp3,"%f %f %f %f \n", (t), Xvibweighted[2], Yvibweighted[2], Zvibweighted[2]); } //updates aZ[0] = aZ[1]; aZ[1] = aZ[2]; aX[0] = aX[1]; aX[1] = aX[2]; aY[0] = aY[1]; aY[1] = aY[2]; Zvibweighted[0] = Zvibweighted[1]; Zvibweighted[1] = Zvibweighted[2]; Xvibweighted[0] = Xvibweighted[1]; Xvibweighted[1] = Xvibweighted[2]; Yvibweighted[0] = Yvibweighted[1]; Yvibweighted[1] = Yvibweighted[2]; //azfilter1Hz } }//Fin Hl fclose(fp); fclose(fp3); ///////////////////////////////////////// //Segund pasada Hh. (High Pass 0.4Hz, Orden 2) strcpy(resultsFile,"/local/W_IV_"); strcpy(resultsFileW,"/local/W2_IV_"); sprintf (NoViajel, "%d", ContadorEnsayos); strcat(resultsFile,NoViajel); strcat(resultsFileW,NoViajel); strcat(resultsFile,".txt"); strcat(resultsFileW,".txt"); //fichero con todos los datos de la función anterior IV_ fp = fopen(resultsFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", resultsFile); } //fichero donde guardaré fp3 = fopen(resultsFileW,"w"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", resultsFileW); } //loop principal. Pasada 2. Hh (High Pass 0.4Hz Orden 2) for(unsigned int i = 0; i<(ContadorDatos-(ndes+nmed)); i++) { 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. //isofilter if ( i==0 ) { Zvibweighted[0] = aXYZ[2]; Xvibweighted[0] = aXYZ[0]; Yvibweighted[0] = aXYZ[1]; aZ[0] =aXYZ[2]; aX[0] =aXYZ[0]; aY[0] =aXYZ[1]; fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[0], Yvibweighted[0], Zvibweighted[0]); }else if ( i==1 ) { Zvibweighted[1] = aXYZ[2]; Xvibweighted[1] = aXYZ[0]; Yvibweighted[1] = aXYZ[1]; aZ[1] =aXYZ[2]; aX[1] =aXYZ[0]; aY[1] =aXYZ[1]; fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[1], Yvibweighted[1], Zvibweighted[1]); }else { aZ[2] = aXYZ[2]; aX[2] = aXYZ[0]; aY[2] = aXYZ[1]; // Zvibweighted[2] = aXYZ[2];//isoFilter_Wk_z_Hh(Zvibweighted[0],Zvibweighted[1],aZ); // Xvibweighted[2] = aXYZ[0];//isoFilter_Wd_xy_Hh(Xvibweighted[0],Xvibweighted[1],aX); //OJO: el filtro High Pass no funciona bien // Yvibweighted[2] = aXYZ[1];//isoFilter_Wd_xy_Hh(Yvibweighted[0],Yvibweighted[1],aY); Zvibweighted[2] = isoFilter_Wk_z_Hh(Zvibweighted[0],Zvibweighted[1],aZ, PasoTiempo); Xvibweighted[2] = isoFilter_Wd_xy_Hh(Xvibweighted[0],Xvibweighted[1],aX, PasoTiempo); Yvibweighted[2] = isoFilter_Wd_xy_Hh(Yvibweighted[0],Yvibweighted[1],aY, PasoTiempo); fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[2], Yvibweighted[2], Zvibweighted[2]); //updates aZ[0] = aZ[1]; aZ[1] = aZ[2]; aX[0] = aX[1]; aX[1] = aX[2]; aY[0] = aY[1]; aY[1] = aY[2]; Zvibweighted[0] = Zvibweighted[1]; Zvibweighted[1] = Zvibweighted[2]; Xvibweighted[0] = Xvibweighted[1]; Xvibweighted[1] = Xvibweighted[2]; Yvibweighted[0] = Yvibweighted[1]; Yvibweighted[1] = Yvibweighted[2]; } }//Fin Hh1 fclose(fp); fclose(fp3); int rmDVA0 = remove(resultsFile); ///////////////////////////////////////// //Tercera pasada Ht. (Ht filtro transicion) strcpy(resultsFile,"/local/W2_IV_"); strcpy(resultsFileW,"/local/W4_IV_"); sprintf (NoViajel, "%d", ContadorEnsayos); strcat(resultsFile,NoViajel); strcat(resultsFileW,NoViajel); strcat(resultsFile,".txt"); strcat(resultsFileW,".txt"); //fichero con todos los datos de la función anterior IV_ fp = fopen(resultsFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", resultsFile); } //fichero donde guardaré fp3 = fopen(resultsFileW,"w"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", resultsFileW); } //loop principal. Pasada 3. Ht (Transicion) for(unsigned int i = 0; i<(ContadorDatos-(ndes+nmed)); i++) { 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. //isofilter if ( i==0 ) { Zvibweighted[0] = aXYZ[2]; Xvibweighted[0] = aXYZ[0]; Yvibweighted[0] = aXYZ[1]; aZ[0] =aXYZ[2]; aX[0] =aXYZ[0]; aY[0] =aXYZ[1]; fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[0], Yvibweighted[0], Zvibweighted[0]); }else if ( i==1 ) { Zvibweighted[1] = aXYZ[2]; Xvibweighted[1] = aXYZ[0]; Yvibweighted[1] = aXYZ[1]; aZ[1] =aXYZ[2]; aX[1] =aXYZ[0]; aY[1] =aXYZ[1]; fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[1], Yvibweighted[1], Zvibweighted[1]); }else { aZ[2] = aXYZ[2]; aX[2] = aXYZ[0]; aY[2] = aXYZ[1]; Zvibweighted[2] = isoFilter_Wk_z_Ht(Zvibweighted[0],Zvibweighted[1],aZ, PasoTiempo); Xvibweighted[2] = isoFilter_Wd_xy_Ht(Xvibweighted[0],Xvibweighted[1],aX, PasoTiempo); Yvibweighted[2] = isoFilter_Wd_xy_Ht(Yvibweighted[0],Yvibweighted[1],aY, PasoTiempo); // Zvibweighted[2] = aXYZ[2]; // Xvibweighted[2] = aXYZ[0]; // Yvibweighted[2] = aXYZ[1]; fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[2], Yvibweighted[2], Zvibweighted[2]); //updates aZ[0] = aZ[1]; aZ[1] = aZ[2]; aX[0] = aX[1]; aX[1] = aX[2]; aY[0] = aY[1]; aY[1] = aY[2]; Zvibweighted[0] = Zvibweighted[1]; Zvibweighted[1] = Zvibweighted[2]; Xvibweighted[0] = Xvibweighted[1]; Xvibweighted[1] = Xvibweighted[2]; Yvibweighted[0] = Yvibweighted[1]; Yvibweighted[1] = Yvibweighted[2]; } }//Fin Ht fclose(fp); fclose(fp3); rmDVA0 = remove(resultsFile); ///////////////////////////////////////// //Cuarta pasada Hs. (Hs filtro subida de escala) strcpy(resultsFile,"/local/W4_IV_"); strcpy(resultsFileW,"/local/W5_IV_"); sprintf (NoViajel, "%d", ContadorEnsayos); strcat(resultsFile,NoViajel); strcat(resultsFileW,NoViajel); strcat(resultsFile,".txt"); strcat(resultsFileW,".txt"); //fichero con todos los datos de la función anterior IV_ fp = fopen(resultsFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", resultsFile); } //fichero donde guardaré fp3 = fopen(resultsFileW,"w"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", resultsFileW); } //loop principal. Pasada 4. Hs (subida escala) for(unsigned int i = 0; i<(ContadorDatos-(ndes+nmed)); i++) { 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. //isofilter if ( i==0 ) { Zvibweighted[0] = aXYZ[2]; Xvibweighted[0] = aXYZ[0]; Yvibweighted[0] = aXYZ[1]; aZ[0] =aXYZ[2]; aX[0] =aXYZ[0]; aY[0] =aXYZ[1]; fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[0], Yvibweighted[0], Zvibweighted[0]); }else if ( i==1 ) { Zvibweighted[1] = aXYZ[2]; Xvibweighted[1] = aXYZ[0]; Yvibweighted[1] = aXYZ[1]; aZ[1] =aXYZ[2]; aX[1] =aXYZ[0]; aY[1] =aXYZ[1]; fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[1], Yvibweighted[1], Zvibweighted[1]); }else { aZ[2] = aXYZ[2]; aX[2] = aXYZ[0]; aY[2] = aXYZ[1]; Zvibweighted[2] = isoFilter_Wk_z_Hs(Zvibweighted[0],Zvibweighted[1],aZ, PasoTiempo); Xvibweighted[2] = aX[2]; Yvibweighted[2] = aY[2]; // Zvibweighted[2] = aXYZ[2]; // Xvibweighted[2] = aX[2]; // Yvibweighted[2] = aY[2]; fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[2], Yvibweighted[2], Zvibweighted[2]); //updates aZ[0] = aZ[1]; aZ[1] = aZ[2]; aX[0] = aX[1]; aX[1] = aX[2]; aY[0] = aY[1]; aY[1] = aY[2]; Zvibweighted[0] = Zvibweighted[1]; Zvibweighted[1] = Zvibweighted[2]; Xvibweighted[0] = Xvibweighted[1]; Xvibweighted[1] = Xvibweighted[2]; Yvibweighted[0] = Yvibweighted[1]; Yvibweighted[1] = Yvibweighted[2]; } }//Fin Ht fclose(fp); fclose(fp3); rmDVA0 = remove(resultsFile); } void calculaStep2(unsigned int ContadorEnsayos, unsigned int ContadorDatos) { //calcula step2 previo filtadro deaZ ya filtrada a 10Hz, a 1Hz esta vez. float t; float dZ,vZ,aZfilter10Hz,jerk,aZ[3]; float azfilter1Hz[3]; //Calculo de step2 const float running_time = 1.0; 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 float azfilter1Hz_1s[2*Ai+1]; //Aceleraciones dentro del periodo de 0.5 segundos para hacer regresion lineal float step2_05s[Ai+1]; //Jerks dentro del periodo de 0.25 for (unsigned int i = 0; i<(2*Ai+1); i++) { azfilter1Hz_1s[i] = 0.0; } for (unsigned int i = 0; i<(Ai+1); i++) { step2_05s[i] = 0.0; } //Archivos de lectura y escritura FILE * fp; FILE * fp2; char NoViajel[2]; char dispVelFile[50]; char velMod[50]; strcpy(dispVelFile,"/local/dvaj_"); strcpy(velMod,"/local/dvaja_"); sprintf (NoViajel, "%d", ContadorEnsayos); strcat(dispVelFile,NoViajel); strcat(velMod,NoViajel); strcat(dispVelFile,".txt"); strcat(velMod,".txt"); fp = fopen(dispVelFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", dispVelFile); } fp2 = fopen(velMod,"w"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", velMod); } //const float fs = (1000000.0/PasoTiempo);//frecuencia de muestreo debe valer 320 //Primera pasada. calcula azfilter1Hz int nDatos = (ContadorDatos-(ndes+nmed)); if (nDatos < 0) {nDatos = 0;} for(unsigned int i = 0; i<nDatos; i++) { //fscanf(fp,"%f %f %f %f %f %f", &t, &dZ, &vZ, &aZ, &aX, &aY);//aZ filtered, aX y aY raw. fscanf(fp,"%f %f %f %f %f", &t, &dZ, &vZ, &aZfilter10Hz, &jerk); // //isofilter if ( i==0 ) { //azfilter1Hz aZ[0] = aZfilter10Hz; azfilter1Hz[0] = aZfilter10Hz; fprintf(fp2,"%f %f %f %f %f %f \n", t, dZ, vZ, aZfilter10Hz, jerk, azfilter1Hz[0]); }else if ( i==1 ) { //azfilter1Hz aZ[1] = aZfilter10Hz; azfilter1Hz[1] = aZfilter10Hz; fprintf(fp2,"%f %f %f %f %f %f \n", t, dZ, vZ, aZfilter10Hz, jerk, azfilter1Hz[1]); }else { //azfilter1Hz aZ[2] = aZfilter10Hz; if ( FrecAdq == 320 ) { azfilter1Hz[2] = toLowPass1HzO2_320(azfilter1Hz[0],azfilter1Hz[1],aZ); }else if ( FrecAdq == 1000 ) { azfilter1Hz[2] = toLowPass1HzO2_1000(azfilter1Hz[0],azfilter1Hz[1],aZ); }else//seguridad { azfilter1Hz[2] = toLowPass1HzO2_320(azfilter1Hz[0],azfilter1Hz[1],aZ); } fprintf(fp2,"%f %f %f %f %f %f \n", t, dZ, vZ, aZfilter10Hz, jerk, azfilter1Hz[2]); //updates aZ[0] = aZ[1]; aZ[1] = aZ[2]; //azfilter1Hz azfilter1Hz[0] = azfilter1Hz[1]; azfilter1Hz[1] = azfilter1Hz[2]; } }//fin pasada fclose(fp); fclose(fp2); //int rmDVA0 = remove(dispVelFile);//elimina dvaj_ //segunda pasada. Calcula step2 strcpy(dispVelFile,"/local/dvaja_"); strcpy(velMod,"/local/dvaj2_"); sprintf (NoViajel, "%d", ContadorEnsayos); strcat(dispVelFile,NoViajel); strcat(velMod,NoViajel); strcat(dispVelFile,".txt"); strcat(velMod,".txt"); fp = fopen(dispVelFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", dispVelFile); } fp2 = fopen(velMod,"w"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", velMod); } if (nDatos < 0) {nDatos = 0;} for(unsigned int i = 0; i<nDatos; i++) { //fscanf(fp,"%f %f %f %f %f %f", &t, &dZ, &vZ, &aZ, &aX, &aY);//aZ filtered, aX y aY raw. fscanf(fp,"%f %f %f %f %f %f", &t, &dZ, &vZ, &aZfilter10Hz, &jerk, &azfilter1Hz[2]); //Calculo de step2 por regresion for(unsigned int j = 0; j<(2*Ai+1); j++) { azfilter1Hz_1s[j] = azfilter1Hz_1s[j+1]; } azfilter1Hz_1s[2*Ai] = azfilter1Hz[2]; for(unsigned int j = 0; j<(Ai+1); j++) { step2_05s[j] = step2_05s[j+1]; } step2_05s[Ai] = RegresionJerk(azfilter1Hz_1s, 2*Ai+1); fprintf(fp2,"%f %f %f %f %f %f \n", t, dZ, vZ, aZfilter10Hz, jerk, step2_05s[0]); } fclose(fp); fclose(fp2); int rmDVA1 = remove(dispVelFile); } void calculaCarNCar(unsigned int ContadorEnsayos, unsigned int ContadorDatos, int PasoTiempo) { float t; float dZ,vZ,aZfilter10Hz,jerk,step2; const float timeStep = (float)(PasoTiempo/1000000.0); const float Atadq = timeStep; //Archivos de lectura y escritura FILE * fp; char NoViajel[2]; char dispVelFile[50]; strcpy(dispVelFile,"/local/dvaj2_"); sprintf (NoViajel, "%d", ContadorEnsayos); strcat(dispVelFile,NoViajel); strcat(dispVelFile,".txt"); fp = fopen(dispVelFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", dispVelFile); } int control(0); vector <int> regiones; int nDatos = (ContadorDatos-(ndes+nmed)); if (nDatos < 0) {nDatos = 0;} for(int i = 0; i<nDatos; i++) { fscanf(fp,"%f %f %f %f %f %f \n", &t, &dZ, &vZ, &aZfilter10Hz, &jerk, &step2);//aZ filtered, aX y aY raw. //Step 3: Identifica slopes con valor mayor que 0.3 m/s^3 if ( (i >= Bo0) && (i <= Bo3) ) { if ((fabs(step2)>=0.3)&&(control==0)) { regiones.push_back(i-((int)(0.5/timeStep)));//correccion indice por retraso step2 en 0.5 segundos OJO // regiones.push_back(i);//correccion indice por retraso step2 en 0.5 segundos OJO control=1; } else if ((fabs(step2)<=0.3)&&(control==1)) { control=0; regiones.push_back(i-1-((int)(0.5/timeStep)));//correccion indice por retraso step2 en 0.5 segundos // regiones.push_back(i-1);//correccion indice por retraso step2 en 0.5 segundos } } }//fin loop AQUI---abajo, cambiar time.at(regiones.at(2*i)) por tiempo sabiendo el paso etc. //Step 4: Add 0.5 s . Hecho en el bucle de abajo. al primero le resto 0.5 y // al segundo se lo sumo. Para los indices como conozco Atadq, sumo y resto // 0.5/Atadq Nregiones = (int)(regiones.size()/2.0); //region [t1 t2 index(t1) index(t1)] if(Nregiones > 0) { region = new float* [Nregiones]; for(unsigned int i = 0; i < Nregiones; i++) region[i] = new float[4]; for (unsigned int i = 0; i < Nregiones; i++) { float factorT(0.5); // while ( ((timeStep*regiones.at(2*i)-factorT)<0) || ((timeStep*regiones.at(2*i +1)+factorT)>time.at(time.size()-1)) ) while ( ((timeStep*regiones.at(2*i)-factorT)<0) || ((timeStep*regiones.at(2*i +1)+factorT)>timeStep*Bo3) ) { factorT = factorT - 0.1; } region[i][0] = timeStep*regiones.at(2*i)-factorT; region[i][2] = regiones.at(2*i)-factorT/Atadq; region[i][1] = timeStep*regiones.at(2*i +1)+factorT; region[i][3] = regiones.at(2*i + 1)+factorT/Atadq; } // Step 5: cada fila de region (=Nregiones) es una region de non-constant // acceleration // Step 6: define regiones de constant acceleration (car) int cont = 0; for (unsigned int i=0; i < (Nregiones-1); i++) { if ( region[i+1][2] > region[i][3] ) { regionconst[cont][0]=region[i][1]; regionconst[cont][1]=region[i+1][0]; regionconst[cont][2]=region[i][3]; regionconst[cont][3]=region[i+1][2]; cont++; } } Nregionesconst = cont; } else { Nregionesconst = 0; } fclose(fp); int rmDVA0 = remove(dispVelFile); } void calculaZXYvibweighted( unsigned int ContadorEnsayos, unsigned int ContadorDatos) { //aux data float Zvibweighted_max(0.0); float Zvibweighted_min(0.0); float Zvibweighted_max_ncar(0.0); float Zvibweighted_min_ncar(0.0); float Xvibweighted_max(0.0); float Xvibweighted_min(0.0); float Yvibweighted_max(0.0); float Yvibweighted_min(0.0); int signoCompara; int signoCompara_ncar; int signoCompara_X; int signoCompara_Y; int signoControl; int signoControl_X; int signoControl_Y; int contadorCeros(0); int contadorCeros_ncar(0); int contadorCeros_X(0); int contadorCeros_Y(0); float difZ(0.0); float difX(0.0); float difY(0.0); int contador_peaktopeak_car(0); int contador_peaktopeak_X(0); int contador_peaktopeak_Y(0); int marca(-1);//-1 = no //read data from file float Xvibweighted; float Yvibweighted; float Zvibweighted; float t; //Archivos de lectura y escritura FILE * fp; FILE * fp2; FILE * fp3; FILE * fp4; char NoViajel[2]; char dispVelFile[50]; char A95file[50]; char A95fileX[50]; char A95fileY[50]; strcpy(dispVelFile,"/local/W5_IV_"); strcpy(A95file,"/local/A95_IV"); strcpy(A95fileX,"/local/AX_IV_"); strcpy(A95fileY,"/local/AY_IV_"); sprintf (NoViajel, "%d", ContadorEnsayos); strcat(dispVelFile,NoViajel); strcat(A95file,NoViajel); strcat(A95fileX,NoViajel); strcat(A95fileY,NoViajel); strcat(dispVelFile,".txt"); strcat(A95file,".txt"); strcat(A95fileX,".txt"); strcat(A95fileY,".txt"); fp = fopen(dispVelFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", dispVelFile); } fp2 = fopen(A95file,"w"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95file); } fp3 = fopen(A95fileX,"w"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileX); } fp4 = fopen(A95fileY,"w"); if(fp4 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileY); } int nDatos = (ContadorDatos-(ndes+nmed)); if (nDatos < 0) {nDatos = 0;} for(unsigned int i = 0; i<nDatos; i++)//-------------------------------------------------------------MAIN LOOP { fscanf(fp,"%f %f %f %f", &t, &Xvibweighted, &Yvibweighted, &Zvibweighted);//lee tiempo y 3 aceleraciones ponderadas X, Y, Z /////////////////////////////////////////////////////////////////////////////////////////////// ////////////////Zvib/////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////// //signo signoControl = (int)(Zvibweighted/fabs(Zvibweighted));//+1 positivo, -1 negativo ////////////////////////////////////////////////////////////////////////////////////// //Constant acceleration Region (subindex=car). Maximum peak to peak y A95 peak to peak //comprueba que esta dentro de alguna de las fronteras if ( Nregionesconst == 0 ) { max_peaktopeak_car_ = 0.0; A95_peaktopeak_car_ = 0.0; }else { for (int j = 0 ; j < Nregionesconst ; j++) { if ( i == ((int)regionconst[j][2]) ) { signoCompara = (int)(Zvibweighted/fabs(Zvibweighted));//+1 positivo, -1 negativo } if ( (((int)regionconst[j][2]) < i) && (i < (int)regionconst[j][3]) ) { if ( (signoControl-signoCompara) != 0 ) { contadorCeros++; signoCompara = signoControl; } if ( (1 <= contadorCeros) && (contadorCeros < 3) ) { if ( Zvibweighted > Zvibweighted_max ) { Zvibweighted_max = Zvibweighted; } if ( Zvibweighted < Zvibweighted_min ) { Zvibweighted_min = Zvibweighted; } }else if (contadorCeros == 3) { contadorCeros = 0; difZ = (Zvibweighted_max - Zvibweighted_min); if ( difZ > max_peaktopeak_car_ ){max_peaktopeak_car_ = difZ;} //almacena para A95 fprintf(fp2,"%f \n", difZ); contador_peaktopeak_car++; Zvibweighted_max = 0.0; Zvibweighted_min = 0.0; } } }//for Nregionesconst }//else ////////////////////////////////////////////////////////////////////////////////////// //Non-constant acceleration Region (subindex=ncar). Maximum peak to peak //comprueba que esta dentro de alguna de las fronteras if ( Nregiones == 0 ) { max_peaktopeak_ncar_ = 0.0; }else { for (int j = 0 ; j < Nregiones ; j++) { if ( i == ((int)region[j][2]) ) { signoCompara_ncar = (int)(Zvibweighted/fabs(Zvibweighted));//+1 positivo, -1 negativo marca = j; } if ( (((int)region[j][2]) < i) && (i < (int)region[j][3]) ) { if ( (signoControl-signoCompara_ncar) != 0 ) { contadorCeros_ncar++; signoCompara_ncar = signoControl; } if ( (1 <= contadorCeros_ncar) && (contadorCeros_ncar < 3))// && (marca == j) ) { if ( (Zvibweighted > Zvibweighted_max_ncar) && (marca == j) ) { Zvibweighted_max_ncar = Zvibweighted; } if ( (Zvibweighted < Zvibweighted_min_ncar) && (marca == j) ) { Zvibweighted_min_ncar = Zvibweighted; } }else { contadorCeros_ncar = 0; //marca = -1; if ( (Zvibweighted_max_ncar - Zvibweighted_min_ncar) > max_peaktopeak_ncar_ ){max_peaktopeak_ncar_ = (Zvibweighted_max_ncar - Zvibweighted_min_ncar);} Zvibweighted_max_ncar = 0.0; Zvibweighted_min_ncar = 0.0; } } }//for Nregiones }//else ////////////////////////////////////////////////////////////////////////////////// ////////Xvib///////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////// //signo signoControl_X = (int)(Xvibweighted/fabs(Xvibweighted));//+1 positivo, -1 negativo if ( i == Bo1 ) { signoCompara_X = (int)(Xvibweighted/fabs(Xvibweighted));//+1 positivo, -1 negativo } if ( Bo1 == Bo2 ) { max_peaktopeak_X_ = 0.0; }else { if (( Bo1 < i) && (i < Bo2 )) { if ( (signoControl_X-signoCompara_X) != 0 ) { contadorCeros_X++; signoCompara_X = signoControl_X; } if ( (1 <= contadorCeros_X) && (contadorCeros_X < 3) ) { if ( Xvibweighted > Xvibweighted_max ) { Xvibweighted_max = Xvibweighted; } if ( Xvibweighted < Xvibweighted_min ) { Xvibweighted_min = Xvibweighted; } }else if (contadorCeros_X == 3) { contadorCeros_X = 0; difX = (Xvibweighted_max - Xvibweighted_min); if ( difX > max_peaktopeak_X_ ){max_peaktopeak_X_ = difX;} //almacena para A95 fprintf(fp3,"%f \n", difX); contador_peaktopeak_X++; Xvibweighted_max = 0.0; Xvibweighted_min = 0.0; } } } ////////////////////////////////////////////////////////////////////////////////// ////////Yvib///////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////// //signo signoControl_Y = (int)(Yvibweighted/fabs(Yvibweighted));//+1 positivo, -1 negativo if ( i == Bo1 ) { signoCompara_Y = (int)(Yvibweighted/fabs(Yvibweighted));//+1 positivo, -1 negativo } if ( Bo1 == Bo2 ) { max_peaktopeak_Y_ = 0.0; }else { if (( Bo1 < i) && (i < Bo2 )) { if ( (signoControl_Y-signoCompara_Y) != 0 ) { contadorCeros_Y++; signoCompara_Y = signoControl_Y; } if ( (1 <= contadorCeros_Y) && (contadorCeros_Y < 3) ) { if ( Yvibweighted > Yvibweighted_max ) { Yvibweighted_max = Yvibweighted; } if ( Yvibweighted < Yvibweighted_min ) { Yvibweighted_min = Yvibweighted; } }else if (contadorCeros_Y == 3) { contadorCeros_Y = 0; difY = (Yvibweighted_max - Yvibweighted_min); if ( difY > max_peaktopeak_Y_ ){max_peaktopeak_Y_ = difY;} //almacena para A95 fprintf(fp4,"%f \n", difY); contador_peaktopeak_Y++; Yvibweighted_max = 0.0; Yvibweighted_min = 0.0; } } } }//fin Loop Principal fclose(fp); if ( fp2 != NULL ) { fclose(fp2); } if ( fp3 != NULL ) { fclose(fp3); } if ( fp4 != NULL ) { fclose(fp4); } ///////////////////////////////////////////////////////////// //Calculo A95s/////////////////////////////////////////////// //Z //abro fichero fp2 = fopen(A95file,"r"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95file); } float maxZvibweighted; int length_maximoZvib = (int)(contador_peaktopeak_car*0.05) + 1; int maxindex(0); vector<float> maximoZvib (length_maximoZvib,0.0); vector<float> diferencia_vec; vector<float>::iterator up; float diferencia; float maximo; int contadorA95(0); //pasada 1 for(unsigned int i = 0; i<contador_peaktopeak_car; i++) { fscanf(fp2,"%f", &maxZvibweighted); for (unsigned int j = 0; j<length_maximoZvib; j++) { diferencia = maxZvibweighted - maximoZvib.at(j); if ( diferencia < 0 ) { diferencia_vec.push_back(0.0); contadorA95++; }else { diferencia_vec.push_back(diferencia); } } if ( contadorA95 == length_maximoZvib ){} else { maximo = *max_element(diferencia_vec.begin(),diferencia_vec.end()); up = find(diferencia_vec.begin(), diferencia_vec.end(), maximo); maxindex = int(up - diferencia_vec.begin()); maximoZvib.erase(maximoZvib.begin() + maxindex ); maximoZvib.insert(maximoZvib.begin() + maxindex ,maxZvibweighted); } contadorA95 = 0; diferencia_vec.clear(); } sort(maximoZvib.begin(),maximoZvib.end());//ascending reverse(maximoZvib.begin(),maximoZvib.end());//descending fclose(fp2); //pasada 2 //abro fichero fp2 = fopen(A95file,"r"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95file); } double resta(1000.0); for(unsigned int i = 0; i<contador_peaktopeak_car; i++) { fscanf(fp2,"%f", &maxZvibweighted); if( ((maximoZvib.at(maximoZvib.size() - 1) - maxZvibweighted) <= resta) && (maxZvibweighted < maximoZvib.at(maximoZvib.size() - 1)) ) { A95_peaktopeak_car_ = maxZvibweighted; resta = maximoZvib.at(maximoZvib.size() - 1) - maxZvibweighted; } } //clear all vectors maximoZvib.clear(); diferencia_vec.clear(); fclose(fp2); //X///////////////////////////////////////// //abro fichero fp3 = fopen(A95fileX,"r"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileX); } float maxXvibweighted; int length_maximoXvib = (int)(contador_peaktopeak_X*0.05) + 1; maxindex = 0; vector<float> maximoXvib (length_maximoXvib,0.0); diferencia = 0.0; maximo =0.0; contadorA95 = 0; if ( fp3 == NULL ) { A95_peaktopeak_X_ = 0.0; }else { //pasada 1 for(unsigned int i = 0; i<contador_peaktopeak_X; i++) { fscanf(fp3,"%f", &maxXvibweighted); for (unsigned int j = 0; j<length_maximoXvib; j++) { diferencia = maxXvibweighted - maximoXvib.at(j); if ( diferencia < 0 ) { diferencia_vec.push_back(0.0); contadorA95++; }else { diferencia_vec.push_back(diferencia); } } if ( contadorA95 == length_maximoXvib ){} else { maximo = *max_element(diferencia_vec.begin(),diferencia_vec.end()); up = find(diferencia_vec.begin(), diferencia_vec.end(), maximo); maxindex = int(up - diferencia_vec.begin()); maximoXvib.erase(maximoXvib.begin() + maxindex ); maximoXvib.insert(maximoXvib.begin() + maxindex ,maxXvibweighted); } contadorA95 = 0; diferencia_vec.clear(); } sort(maximoXvib.begin(),maximoXvib.end());//ascending reverse(maximoXvib.begin(),maximoXvib.end());//descending fclose(fp3); //pasada 2 //abro fichero fp3 = fopen(A95fileX,"r"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileX); } resta = 1000.0; for(unsigned int i = 0; i<contador_peaktopeak_X; i++) { fscanf(fp3,"%f", &maxXvibweighted); if( ((maximoXvib.at(maximoXvib.size() - 1) - maxXvibweighted) <= resta) && (maxXvibweighted < maximoXvib.at(maximoXvib.size() - 1)) ) { A95_peaktopeak_X_ = maxXvibweighted; resta = maximoXvib.at(maximoXvib.size() - 1) - maxXvibweighted; } } //clear all vectors maximoXvib.clear(); diferencia_vec.clear(); } fclose(fp3); //Y///////////////////////////////////////// //abro fichero fp4 = fopen(A95fileY,"r"); if(fp4 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileY); } float maxYvibweighted; int length_maximoYvib = (int)(contador_peaktopeak_Y*0.05) + 1; maxindex = 0; vector<float> maximoYvib (length_maximoYvib,0.0); diferencia = 0.0; maximo =0.0; contadorA95 = 0; if ( fp4 == NULL ) { A95_peaktopeak_Y_ = 0.0; }else { //pasada 1 for(unsigned int i = 0; i<contador_peaktopeak_Y; i++) { fscanf(fp4,"%f", &maxYvibweighted); for (unsigned int j = 0; j<length_maximoYvib; j++) { diferencia = maxYvibweighted - maximoYvib.at(j); if ( diferencia < 0 ) { diferencia_vec.push_back(0.0); contadorA95++; }else { diferencia_vec.push_back(diferencia); } } if ( contadorA95 == length_maximoYvib ){} else { maximo = *max_element(diferencia_vec.begin(),diferencia_vec.end()); up = find(diferencia_vec.begin(), diferencia_vec.end(), maximo); maxindex = int(up - diferencia_vec.begin()); maximoYvib.erase(maximoYvib.begin() + maxindex ); maximoYvib.insert(maximoYvib.begin() + maxindex ,maxYvibweighted); } contadorA95 = 0; diferencia_vec.clear(); } sort(maximoYvib.begin(),maximoYvib.end());//ascending reverse(maximoYvib.begin(),maximoYvib.end());//descending fclose(fp4); //pasada 2 //abro fichero fp4 = fopen(A95fileY,"r"); if(fp4 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileY); } resta = 1000.0; for(unsigned int i = 0; i<contador_peaktopeak_Y; i++) { fscanf(fp4,"%f", &maxYvibweighted); if( ((maximoYvib.at(maximoYvib.size() - 1) - maxYvibweighted) <= resta) && (maxYvibweighted < maximoYvib.at(maximoYvib.size() - 1)) ) { A95_peaktopeak_Y_ = maxYvibweighted; resta = maximoYvib.at(maximoYvib.size() - 1) - maxYvibweighted; } } //clear all vectors maximoYvib.clear(); diferencia_vec.clear(); } fclose(fp4); //remove files int rmDVA0 = remove(A95file); if(rmDVA0 != 0) { display.Clear();display.SetXY(0,0); display.printf("Error removing %s file\n", A95file); } int rmDVA1 = remove(A95fileX); if(rmDVA1 != 0) { display.Clear();display.SetXY(0,0); display.printf("Error removing %s file\n", A95fileX); } int rmDVA2 = remove(A95fileY); if(rmDVA2 != 0) { display.Clear();display.SetXY(0,0); display.printf("Error removing %s file\n", A95fileY); } int rmDVA3 = remove(dispVelFile); if(rmDVA3 != 0) { display.Clear();display.SetXY(0,0); display.printf("Error removing %s file\n", dispVelFile); } return; } float RegresionJerk(float * aZ, unsigned int ndat) { //Media de las aceleraciones float aZm = 0.0; for(unsigned int j = 0; j<ndat; j++) { aZm += aZ[j]; } aZm = aZm/(float)ndat; //Pendiente por minimos cuadrados float b; //Numerador y denominador en expresion de pendiente float numer = 0.0; float denom = 0.0; int nn; float At; for(int j = 0; j<ndat; j++) { nn = j-((ndat/2)); At = (float)nn*(1.0e-6)*(float)(PasoTiempo); denom = denom + At*At; numer = numer + At*(aZ[j]-aZm); } b = numer/denom; return b; } void DisplayResultados1(void) { for(int i = 0; i<2; i++) { display.Clear(); if(TRADUCE==0) { display.SetXY(0,0); display.printf("Datos velocidad"); display.SetXY(0,1); display.printf("aceleracion jerk"); }else { display.SetXY(0,0); display.printf("Velocity jerk"); display.SetXY(0,1); display.printf("acceleration"); } wait(2.0); display.Clear(); display.SetXY(0,0); display.printf("VNom = %4.2f m/s \n",vNom); display.SetXY(0,1); display.printf("Vmax = %4.2f m/s \n", Vmax); wait(2.0); display.Clear(); display.SetXY(0,0); display.printf("amax = %4.2f m/s2\n", amax); display.SetXY(0,1); display.printf("dmax = %4.2f m/s2\n", dmax); wait(2.0); display.Clear(); display.SetXY(0,0); display.printf("jmax = %4.2f m/s3\n", jerkmax); display.SetXY(0,1); display.printf("VA95 = %4.2f m/s \n", VA95); wait(2.0); display.Clear(); display.SetXY(0,0); display.printf("aA95 = %4.2f m/s2\n", aA95); display.SetXY(0,1); display.printf("dA95 = %4.2f m/s2\n", dA95); wait(2.0); } } void DisplayResultados2(void) { for(int i = 0; i<2; i++) { display.Clear(); if(TRADUCE==0) { display.SetXY(0,0); display.printf("Datos confort"); display.SetXY(0,0); display.printf("vibracion Z,X,Y:"); }else { display.SetXY(0,0); display.printf("Comfort info"); display.SetXY(0,0); display.printf("vibration Z,X,Y:"); } wait(2.0); display.Clear(); display.SetXY(0,0); display.printf("Zmaxca=%4.2f m/s2 \n", max_peaktopeak_car_); display.SetXY(0,1); display.printf("ZA95ca=%4.2f m/s2 \n", A95_peaktopeak_car_); wait(2.0); display.Clear(); display.SetXY(0,0); display.printf("Zmaxnca=%4.2f m/s2 \n", max_peaktopeak_ncar_); wait(2.0); display.Clear(); display.SetXY(0,0); display.printf("Xmax = %4.2f m/s2\n", max_peaktopeak_X_); display.SetXY(0,1); display.printf("XA95 = %4.2f m/s2\n", A95_peaktopeak_X_); wait(2.0); display.Clear(); display.SetXY(0,0); display.printf("Ymax = %4.2f m/s2\n", max_peaktopeak_Y_); display.SetXY(0,1); display.printf("YA95 = %4.2f m/s2\n", A95_peaktopeak_Y_); wait(2.0); } } void FileResultados( unsigned int ContadorEnsayos ) { FILE * fp; char NoViaje[2]; char resultsFile[50]; strcpy(resultsFile,"/local/ISO_"); sprintf (NoViaje, "%d", ContadorEnsayos); strcat(resultsFile,NoViaje); strcat(resultsFile,".txt"); fp = fopen(resultsFile,"w"); if(fp == NULL) { display.printf("Could not open file %s for read\n", resultsFile); } 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_); //fprintf(fp,"%d %d %d \n", Bo1, Bo2, Bo3); fclose(fp); } void LeeAcelerometroBuffer() { bma180.ReadAccels_BMA180(readings); aaX = (int16_t)readings[0]; aaY = (int16_t)readings[1]; aaZ = (int16_t)readings[2]; //tt_inst = (int16_t)timer.read_us(); aX = (-1.0)*((float)aaX-(-103.23))*0.0018; aY = (-1.0)*((float)aaY-(57.21))*0.0018; aZ = (-1.0)*((float)aaZ-(701.1184))*0.0018097570403; //t_inst = (float)tt_inst; //buff_t.put(t_inst); buff_aX.put(aX); buff_aY.put(aY); buff_aZ.put(aZ); } void GuardaDatosBuffer() { bool empty = buff_aX.isEmpty(); int wi = buff_aX.getWritingIndex(); int ri = buff_aX.getReadingIndex(); int dif; //Diferencia entre writting index y riding index. El buffer se llena cuando esta diferencia alcanza el tamaño del buffer float tiempo; //Tiempo en segundos if((wi-ri)>0) { dif = wi-ri; } else { dif = sizeBuff+wi-ri; } if(empty == false) { aXr = buff_aX.get(); aYr = buff_aY.get(); aZr = buff_aZ.get(); tiempo = ((float)ContadorDatos)*((float)PasoTiempo)/1.0e6; //pc.printf("%d %f %f %f %d\n", ContadorDatos*PasoTiempo, aXr, aYr, aZr, dif); fprintf(fp,"%f %f %f %f %d\n", tiempo, aXr, aYr, aZr, dif); ContadorDatos++; } } void ParamDependFrecAdq() { if(FrecAdq == 320) { PasoTiempo = 3125; gain = 1.184456202e+02; c_butt1 = -0.7575469445; c_butt2 = 1.7237761728; ndes = 100; nmed = 180; } if(FrecAdq == 1000) { PasoTiempo = 1000; gain = 1.058546241e+03; c_butt1 = -0.9149758348; c_butt2 = 1.9111970674; ndes = 300; nmed = 600; } /// Para fadq = 100 Hz, PasoTiempo = 10000 /// //float gain = 1.482463775e+01; //float c_butt1 = -0.4128015981; //float c_butt2 = 1.1429805025; //unsigned int ndes = 30; //unsigned int nmed = 60; /// Para fadq = 328 Hz, PasoTiempo = 3050 /// //float gain = 1.239670776e+02; //float c_butt1 = -0.7626121550; //float c_butt2 = 1.7303455236; //unsigned int ndes = 100; //unsigned int nmed = 180; /// Para fadq = 320 Hz, PasoTiempo = 3125 /// //const float gain = 1.184456202e+02; //const float c_butt1 = -0.7575469445; //const float c_butt2 = 1.7237761728; //unsigned int ndes = 100; //unsigned int nmed = 180; /// Para fadq = 1000 Hz, PasoTiempo = 1000 /// //const float gain = 1.058546241e+03; //const float c_butt1 = -0.9149758348; //const float c_butt2 = 1.9111970674; //const unsigned int ndes = 300; //const unsigned int nmed = 600; }