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