virtualmech
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
--- /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