Esta versión v6 pasa a ser el nuevo master. Funciona correctamente
Dependencies: ADXL345 Display1602 MSCFileSystem SDFileSystem mbed FATFileSystem
Diff: vmConfort.cpp
- Revision:
- 0:a5367bd4e404
- Child:
- 1:ff391cfd0e77
diff -r 000000000000 -r a5367bd4e404 vmConfort.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/vmConfort.cpp Tue May 20 15:11:16 2014 +0000 @@ -0,0 +1,3176 @@ +#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; +} \ No newline at end of file