Esta versión v6 pasa a ser el nuevo master. Funciona correctamente
Dependencies: ADXL345 Display1602 MSCFileSystem SDFileSystem mbed FATFileSystem
vmConfort.cpp@1:ff391cfd0e77, 2014-06-04 (annotated)
- Committer:
- jjmedina
- Date:
- Wed Jun 04 15:21:04 2014 +0000
- Revision:
- 1:ff391cfd0e77
- Parent:
- 0:a5367bd4e404
- Child:
- 2:cc4a43d806e2
Se modifica el baudrate para que lleguen m?s datos en modo PC. Se ha comprobado que mejora la velocidad de adquisici?n en modo PC. Ahora se llega a recoger hasta 2 medidas en 8 ms.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jjmedina | 0:a5367bd4e404 | 1 | #include "mbed.h" |
jjmedina | 0:a5367bd4e404 | 2 | #include "Display1602.h" |
jjmedina | 0:a5367bd4e404 | 3 | //#include "SDFileSystem.h" |
jjmedina | 0:a5367bd4e404 | 4 | #include "MSCFileSystem.h" |
jjmedina | 0:a5367bd4e404 | 5 | |
jjmedina | 0:a5367bd4e404 | 6 | #include <iostream> |
jjmedina | 0:a5367bd4e404 | 7 | #include <math.h> |
jjmedina | 0:a5367bd4e404 | 8 | #include <stdio.h> |
jjmedina | 0:a5367bd4e404 | 9 | #include <string.h> |
jjmedina | 0:a5367bd4e404 | 10 | #include <list> |
jjmedina | 0:a5367bd4e404 | 11 | #include <vector> |
jjmedina | 0:a5367bd4e404 | 12 | #include <algorithm> |
jjmedina | 0:a5367bd4e404 | 13 | |
jjmedina | 0:a5367bd4e404 | 14 | #include "filters.h" |
jjmedina | 0:a5367bd4e404 | 15 | #include "BMA180.h" |
jjmedina | 0:a5367bd4e404 | 16 | //#include "ADXL345.h" |
jjmedina | 0:a5367bd4e404 | 17 | #include "RingBuffer.h" |
jjmedina | 0:a5367bd4e404 | 18 | |
jjmedina | 0:a5367bd4e404 | 19 | using namespace std; |
jjmedina | 0:a5367bd4e404 | 20 | |
jjmedina | 0:a5367bd4e404 | 21 | |
jjmedina | 0:a5367bd4e404 | 22 | Timer timer; |
jjmedina | 0:a5367bd4e404 | 23 | |
jjmedina | 0:a5367bd4e404 | 24 | int TiempoInicio = 0; |
jjmedina | 0:a5367bd4e404 | 25 | int TiempoFin = 0; |
jjmedina | 0:a5367bd4e404 | 26 | |
jjmedina | 0:a5367bd4e404 | 27 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 28 | /// CONSTANTES DEPENDIENTES DE LA FRECUENCIA DE ADQUISICION (sus valores se establecen en ParamDependFrecAdq()) /// |
jjmedina | 0:a5367bd4e404 | 29 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 30 | |
jjmedina | 0:a5367bd4e404 | 31 | unsigned int PasoTiempo; //Paso de tiempo de adquisicion en [us]. Frecuencia de adquisicion = 1/PasoTiemo |
jjmedina | 0:a5367bd4e404 | 32 | float gain; // CONSTANTE DEL FILTRO DE BUTTERWORTH PASO BAJO 10 HZ |
jjmedina | 0:a5367bd4e404 | 33 | float c_butt1; // CONSTANTE DEL FILTRO DE BUTTERWORTH PASO BAJO 10 HZ |
jjmedina | 0:a5367bd4e404 | 34 | float c_butt2; // CONSTANTE DEL FILTRO DE BUTTERWORTH PASO BAJO 10 HZ |
jjmedina | 0:a5367bd4e404 | 35 | unsigned int ndes; //Puntos que se descartan al principio por el Filtro Butterworth |
jjmedina | 0:a5367bd4e404 | 36 | unsigned int nmed; //Puntos para calcular la media de aZ al principio del viaje despues de los puntos descartados |
jjmedina | 0:a5367bd4e404 | 37 | |
jjmedina | 0:a5367bd4e404 | 38 | int nDatosPorPaso = 1; |
jjmedina | 0:a5367bd4e404 | 39 | |
jjmedina | 0:a5367bd4e404 | 40 | ///////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 41 | /// DISPOSITIVOS, PERIFERICOS /// |
jjmedina | 0:a5367bd4e404 | 42 | ///////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 43 | |
jjmedina | 0:a5367bd4e404 | 44 | /// ACELEROMETRO ANALOGICO /// |
jjmedina | 0:a5367bd4e404 | 45 | |
jjmedina | 0:a5367bd4e404 | 46 | //AnalogIn Pin_aX(p15); |
jjmedina | 0:a5367bd4e404 | 47 | //AnalogIn Pin_aY(p16); |
jjmedina | 0:a5367bd4e404 | 48 | //AnalogIn Pin_aZ(p17); |
jjmedina | 0:a5367bd4e404 | 49 | |
jjmedina | 0:a5367bd4e404 | 50 | /// ACELEROMETRO DIGITAL Bosch/// |
jjmedina | 0:a5367bd4e404 | 51 | |
jjmedina | 0:a5367bd4e404 | 52 | BMA180 bma180(p5,p6,p7,p8); |
jjmedina | 0:a5367bd4e404 | 53 | |
jjmedina | 0:a5367bd4e404 | 54 | /// ACELEROMETRO DIGITAL AnalogDevices ADXL345/// |
jjmedina | 0:a5367bd4e404 | 55 | |
jjmedina | 0:a5367bd4e404 | 56 | //ADXL345 adxl345(p5, p6, p7, p8); |
jjmedina | 0:a5367bd4e404 | 57 | |
jjmedina | 0:a5367bd4e404 | 58 | Serial pc(USBTX, USBRX); // tx, rx |
jjmedina | 0:a5367bd4e404 | 59 | |
jjmedina | 0:a5367bd4e404 | 60 | /// TARJETA SD /// |
jjmedina | 0:a5367bd4e404 | 61 | |
jjmedina | 0:a5367bd4e404 | 62 | //SDFileSystem sd(p11, p12, p13, p14, "local"); |
jjmedina | 0:a5367bd4e404 | 63 | |
jjmedina | 0:a5367bd4e404 | 64 | /// PEN DRIVE /// |
jjmedina | 0:a5367bd4e404 | 65 | |
jjmedina | 0:a5367bd4e404 | 66 | MSCFileSystem msc("local"); |
jjmedina | 0:a5367bd4e404 | 67 | |
jjmedina | 0:a5367bd4e404 | 68 | /// MEMORIA FLASH LOCAL DE LA MBED /// |
jjmedina | 0:a5367bd4e404 | 69 | |
jjmedina | 0:a5367bd4e404 | 70 | LocalFileSystem local("mbedFlash"); // Create the local filesystem under the name "local" |
jjmedina | 0:a5367bd4e404 | 71 | |
jjmedina | 0:a5367bd4e404 | 72 | /// LEDS /// |
jjmedina | 0:a5367bd4e404 | 73 | |
jjmedina | 0:a5367bd4e404 | 74 | DigitalOut red_LED(p30); |
jjmedina | 0:a5367bd4e404 | 75 | DigitalOut green_LED(p29); |
jjmedina | 0:a5367bd4e404 | 76 | |
jjmedina | 0:a5367bd4e404 | 77 | /// PUSH BUTTONS /// |
jjmedina | 0:a5367bd4e404 | 78 | |
jjmedina | 0:a5367bd4e404 | 79 | DigitalOut red_PUSH(p9); |
jjmedina | 0:a5367bd4e404 | 80 | DigitalOut green_PUSH(p10); |
jjmedina | 0:a5367bd4e404 | 81 | |
jjmedina | 0:a5367bd4e404 | 82 | /// DISPLAY LCD /// |
jjmedina | 0:a5367bd4e404 | 83 | |
jjmedina | 0:a5367bd4e404 | 84 | //Display1602 display(p18,p19,p20,p28,p27,p26,p25,p24,p23,p22,p21); |
jjmedina | 0:a5367bd4e404 | 85 | Display1602 display(p18,p19,p20,p21,p22,p23,p24,p25,p26,p27,p28); |
jjmedina | 0:a5367bd4e404 | 86 | |
jjmedina | 0:a5367bd4e404 | 87 | //void InterpolaMedidasPerdidas(unsigned int, unsigned int); |
jjmedina | 0:a5367bd4e404 | 88 | void FiltradoPeriodosIntegracion(unsigned int, unsigned int); |
jjmedina | 0:a5367bd4e404 | 89 | void DatosConfort(unsigned int, unsigned int, int); |
jjmedina | 0:a5367bd4e404 | 90 | float RegresionJerk(float *, unsigned int); |
jjmedina | 0:a5367bd4e404 | 91 | void calcula_n05n95_Boveloc_jerkmax(unsigned int , unsigned int, int ); |
jjmedina | 0:a5367bd4e404 | 92 | void calcula_A95_V_acel_decel(unsigned int, unsigned int, int); |
jjmedina | 0:a5367bd4e404 | 93 | void vibraReadFilter(unsigned int , unsigned int ); |
jjmedina | 0:a5367bd4e404 | 94 | |
jjmedina | 0:a5367bd4e404 | 95 | void calculaCarNCar(unsigned int , unsigned int, int ); |
jjmedina | 0:a5367bd4e404 | 96 | void calculaStep2(unsigned int , unsigned int ); |
jjmedina | 0:a5367bd4e404 | 97 | |
jjmedina | 0:a5367bd4e404 | 98 | void calculaZXYvibweighted( unsigned int , unsigned int); |
jjmedina | 0:a5367bd4e404 | 99 | |
jjmedina | 0:a5367bd4e404 | 100 | void DisplayResultados1(void); |
jjmedina | 0:a5367bd4e404 | 101 | |
jjmedina | 0:a5367bd4e404 | 102 | void DisplayResultados2(void); |
jjmedina | 0:a5367bd4e404 | 103 | |
jjmedina | 0:a5367bd4e404 | 104 | void FileResultados( unsigned int ); |
jjmedina | 0:a5367bd4e404 | 105 | |
jjmedina | 0:a5367bd4e404 | 106 | unsigned int nArr, nPar, nV0, nVfin; //Instantes (orden en el vector de datos) caracteristicos del viaje |
jjmedina | 0:a5367bd4e404 | 107 | float t_Arr, t_V0, t_Vfin, t_Par; //Instantes (valores del tiempo) caracteristicos del viaje |
jjmedina | 0:a5367bd4e404 | 108 | float vArr = 0.0; //Velocidad que se calcula por integración en el instante de arranque (debe ser 0) |
jjmedina | 0:a5367bd4e404 | 109 | float vPar = 0.0; //Velocidad que se calcula por integración en el instante de parada (debe ser 0) |
jjmedina | 0:a5367bd4e404 | 110 | |
jjmedina | 0:a5367bd4e404 | 111 | float vZmFin = 0.0; //Media de vZ al final del viaje (debe ser zero pero no lo es) |
jjmedina | 0:a5367bd4e404 | 112 | float tFin; //Instante final |
jjmedina | 0:a5367bd4e404 | 113 | |
jjmedina | 0:a5367bd4e404 | 114 | float aZm0 = 0.0; //Aceleracion media de referencia en instantes iniciales en dirección vertical Z |
jjmedina | 0:a5367bd4e404 | 115 | float aYm0 = 0.0; //Aceleracion media de referencia en instantes iniciales en dirección lateral X |
jjmedina | 0:a5367bd4e404 | 116 | float aXm0 = 0.0; //Aceleracion media de referencia en instantes iniciales en dirección lateral Y |
jjmedina | 0:a5367bd4e404 | 117 | |
jjmedina | 0:a5367bd4e404 | 118 | ////////////////////////// |
jjmedina | 0:a5367bd4e404 | 119 | /// MEDIDAS EN DISPLAY /// |
jjmedina | 0:a5367bd4e404 | 120 | ////////////////////////// |
jjmedina | 0:a5367bd4e404 | 121 | |
jjmedina | 0:a5367bd4e404 | 122 | float vNom = 0.0; //Velocidad nominal |
jjmedina | 0:a5367bd4e404 | 123 | float amax = 0.0; |
jjmedina | 0:a5367bd4e404 | 124 | float dmax = 0.0; |
jjmedina | 0:a5367bd4e404 | 125 | float Vmax = 0.0; |
jjmedina | 0:a5367bd4e404 | 126 | float jerkmax = 0.0; |
jjmedina | 0:a5367bd4e404 | 127 | float VA95(0.0); |
jjmedina | 0:a5367bd4e404 | 128 | float aA95(0.0); |
jjmedina | 0:a5367bd4e404 | 129 | float dA95(0.0); |
jjmedina | 0:a5367bd4e404 | 130 | |
jjmedina | 0:a5367bd4e404 | 131 | //por pantalla |
jjmedina | 0:a5367bd4e404 | 132 | float max_peaktopeak_car_(0.0); |
jjmedina | 0:a5367bd4e404 | 133 | float A95_peaktopeak_car_(0.0); |
jjmedina | 0:a5367bd4e404 | 134 | float max_peaktopeak_ncar_(0.0); |
jjmedina | 0:a5367bd4e404 | 135 | float max_peaktopeak_X_(0.0); |
jjmedina | 0:a5367bd4e404 | 136 | float A95_peaktopeak_X_(0.0); |
jjmedina | 0:a5367bd4e404 | 137 | float max_peaktopeak_Y_(0.0); |
jjmedina | 0:a5367bd4e404 | 138 | float A95_peaktopeak_Y_(0.0); |
jjmedina | 0:a5367bd4e404 | 139 | |
jjmedina | 0:a5367bd4e404 | 140 | float d_arr, d_par; //Desplazamientos medidos en instantes de arranque y de parada |
jjmedina | 0:a5367bd4e404 | 141 | |
jjmedina | 0:a5367bd4e404 | 142 | unsigned int Bo0, Bo1, Bo2, Bo3; //Limites de los periodos de calculo de norma ISO |
jjmedina | 0:a5367bd4e404 | 143 | unsigned int Boacel; //Instante donde deja de acelerar para empezar a decelerar |
jjmedina | 0:a5367bd4e404 | 144 | |
jjmedina | 0:a5367bd4e404 | 145 | float Vmax1 = 0.0; |
jjmedina | 0:a5367bd4e404 | 146 | float Vmax2 = 0.0; |
jjmedina | 0:a5367bd4e404 | 147 | unsigned int n05Vmax1(0);unsigned int n95Vmax1(0);unsigned int n05Vmax2(0);unsigned int n95Vmax2(0); |
jjmedina | 0:a5367bd4e404 | 148 | unsigned int Bo_veloc_acel(0); |
jjmedina | 0:a5367bd4e404 | 149 | unsigned int Bo_veloc_decel(0); |
jjmedina | 0:a5367bd4e404 | 150 | |
jjmedina | 0:a5367bd4e404 | 151 | int Nregionesconst(0); |
jjmedina | 0:a5367bd4e404 | 152 | int Nregiones(0); |
jjmedina | 0:a5367bd4e404 | 153 | float **region; |
jjmedina | 0:a5367bd4e404 | 154 | float regionconst[20][4]; //50 porque no sabemos el numero de filas. RESOLVER MEJOR, JUAN 04-05-2012. |
jjmedina | 0:a5367bd4e404 | 155 | |
jjmedina | 0:a5367bd4e404 | 156 | //////////////////////// |
jjmedina | 0:a5367bd4e404 | 157 | /// IDIOMA DE LA GUI /// |
jjmedina | 0:a5367bd4e404 | 158 | //////////////////////// |
jjmedina | 0:a5367bd4e404 | 159 | |
jjmedina | 0:a5367bd4e404 | 160 | unsigned int TRADUCE = 0; // TRADUCE = 0 gui en español, TRADUCE = 1, traduce gui al inglés |
jjmedina | 0:a5367bd4e404 | 161 | unsigned int SeleccionIdioma = 0; // Vale 1 cuando ya se ha seleccionado el idioma |
jjmedina | 0:a5367bd4e404 | 162 | |
jjmedina | 0:a5367bd4e404 | 163 | ///////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 164 | /// FRECUENCIA DE ADQUISICION /// |
jjmedina | 0:a5367bd4e404 | 165 | ///////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 166 | |
jjmedina | 0:a5367bd4e404 | 167 | unsigned int FrecAdq = 0; //La frecuencia de adquisición puede ser 320 Hz (FrecAdq = 320) o 1000 Hz (FrecAdq = 1000) |
jjmedina | 0:a5367bd4e404 | 168 | unsigned int SeleccionFrecAdq = 0; // Vale 1 cuando ya se ha seleccionado la frecuencia de adquisicion |
jjmedina | 0:a5367bd4e404 | 169 | |
jjmedina | 0:a5367bd4e404 | 170 | |
jjmedina | 0:a5367bd4e404 | 171 | ////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 172 | /// ALMACENAMIENTO DE DATOS CON BUFFER /// |
jjmedina | 0:a5367bd4e404 | 173 | ////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 174 | |
jjmedina | 0:a5367bd4e404 | 175 | void LeeAcelerometroBuffer(); |
jjmedina | 0:a5367bd4e404 | 176 | void GuardaDatosBuffer(); |
jjmedina | 0:a5367bd4e404 | 177 | |
jjmedina | 0:a5367bd4e404 | 178 | int readings[3] = {0, 0, 0}; //Lectura aceleraciones sensor digital |
jjmedina | 0:a5367bd4e404 | 179 | float aX, aY, aZ; //Medidas de aceleracion |
jjmedina | 0:a5367bd4e404 | 180 | int16_t aaX, aaY, aaZ; //Medidas de aceleracion y tiempo |
jjmedina | 0:a5367bd4e404 | 181 | |
jjmedina | 0:a5367bd4e404 | 182 | float aXr, aYr, aZr; //Tiempo y medidas de aceleracion leidos del buffer |
jjmedina | 0:a5367bd4e404 | 183 | unsigned int contDatos = 0; //Contador de datos |
jjmedina | 0:a5367bd4e404 | 184 | |
jjmedina | 0:a5367bd4e404 | 185 | int sizeBuff = 500; //Tamaño de los buffer que guardan los datos |
jjmedina | 0:a5367bd4e404 | 186 | |
jjmedina | 0:a5367bd4e404 | 187 | Ticker tickerLectura; |
jjmedina | 0:a5367bd4e404 | 188 | |
jjmedina | 0:a5367bd4e404 | 189 | Buffer buff_aX(sizeBuff); |
jjmedina | 0:a5367bd4e404 | 190 | Buffer buff_aY(sizeBuff); |
jjmedina | 0:a5367bd4e404 | 191 | Buffer buff_aZ(sizeBuff); |
jjmedina | 0:a5367bd4e404 | 192 | |
jjmedina | 0:a5367bd4e404 | 193 | FILE * fp; |
jjmedina | 0:a5367bd4e404 | 194 | FILE * fp2; |
jjmedina | 0:a5367bd4e404 | 195 | |
jjmedina | 0:a5367bd4e404 | 196 | unsigned int ContadorEnsayos = 0; |
jjmedina | 0:a5367bd4e404 | 197 | unsigned int ContadorDatos = 0; |
jjmedina | 0:a5367bd4e404 | 198 | unsigned int ContadorMedidas = 0; |
jjmedina | 0:a5367bd4e404 | 199 | |
jjmedina | 0:a5367bd4e404 | 200 | void ParamDependFrecAdq(void); |
jjmedina | 0:a5367bd4e404 | 201 | |
jjmedina | 0:a5367bd4e404 | 202 | int main() { |
jjmedina | 1:ff391cfd0e77 | 203 | pc.baud(112500); |
jjmedina | 0:a5367bd4e404 | 204 | |
jjmedina | 0:a5367bd4e404 | 205 | timer.start(); |
jjmedina | 0:a5367bd4e404 | 206 | |
jjmedina | 0:a5367bd4e404 | 207 | bool datosPC_green = false; //true = envia datos a PC, false = Guarda datos en tarjeta SD |
jjmedina | 0:a5367bd4e404 | 208 | bool ensayo_red = false; //true = Comienza la medida, false = termina la medida |
jjmedina | 0:a5367bd4e404 | 209 | |
jjmedina | 0:a5367bd4e404 | 210 | char NoViaje[2]; |
jjmedina | 0:a5367bd4e404 | 211 | |
jjmedina | 0:a5367bd4e404 | 212 | ////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 213 | /// ESTABLECE LOS OFFSETS DEL ACELEROMETRO /// |
jjmedina | 0:a5367bd4e404 | 214 | ////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 215 | |
jjmedina | 0:a5367bd4e404 | 216 | char offsets[6]; |
jjmedina | 0:a5367bd4e404 | 217 | |
jjmedina | 0:a5367bd4e404 | 218 | offsets[0]= 0xAE; //x_msb |
jjmedina | 0:a5367bd4e404 | 219 | offsets[1]= 0x9E; //y_msb |
jjmedina | 0:a5367bd4e404 | 220 | offsets[2]= 0x95; //z_msb |
jjmedina | 0:a5367bd4e404 | 221 | |
jjmedina | 0:a5367bd4e404 | 222 | offsets[3]= 0x0C; //x_lsb |
jjmedina | 0:a5367bd4e404 | 223 | offsets[4]= 0x04; //y_lsb |
jjmedina | 0:a5367bd4e404 | 224 | offsets[5]= 0x0C; //z_lsb |
jjmedina | 0:a5367bd4e404 | 225 | |
jjmedina | 0:a5367bd4e404 | 226 | |
jjmedina | 0:a5367bd4e404 | 227 | //bma180.AcelerometroLeeOffsets(readings); |
jjmedina | 0:a5367bd4e404 | 228 | //pc.printf("%d %d %d\n", readings[0], readings[1], readings[2]); |
jjmedina | 0:a5367bd4e404 | 229 | //bma180.AcelerometroEscribeOffsets(offsets); |
jjmedina | 0:a5367bd4e404 | 230 | //bma180.AcelerometroLeeOffsets(readings); |
jjmedina | 0:a5367bd4e404 | 231 | //pc.printf("%d %d %d\n", readings[0], readings[1], readings[2]); |
jjmedina | 0:a5367bd4e404 | 232 | |
jjmedina | 0:a5367bd4e404 | 233 | ///////////////////////////// |
jjmedina | 0:a5367bd4e404 | 234 | /// APAGA EL ACELEROMETRO /// |
jjmedina | 0:a5367bd4e404 | 235 | ///////////////////////////// |
jjmedina | 0:a5367bd4e404 | 236 | |
jjmedina | 0:a5367bd4e404 | 237 | bma180.AcelerometroSleep(); |
jjmedina | 0:a5367bd4e404 | 238 | |
jjmedina | 0:a5367bd4e404 | 239 | ///////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 240 | /// SELECCION DE IDIOMA DE LA GUI /// |
jjmedina | 0:a5367bd4e404 | 241 | ///////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 242 | |
jjmedina | 0:a5367bd4e404 | 243 | display.Clear(); display.SetXY(0,0); display.printf("English: yellow"); display.SetXY(0,1); display.printf("Castellano: rojo"); |
jjmedina | 0:a5367bd4e404 | 244 | |
jjmedina | 0:a5367bd4e404 | 245 | while(SeleccionIdioma == 0) |
jjmedina | 0:a5367bd4e404 | 246 | { |
jjmedina | 0:a5367bd4e404 | 247 | if(red_PUSH.read() == 1) |
jjmedina | 0:a5367bd4e404 | 248 | { |
jjmedina | 0:a5367bd4e404 | 249 | TRADUCE = 0;//traduce gui al español |
jjmedina | 0:a5367bd4e404 | 250 | SeleccionIdioma = 1; |
jjmedina | 0:a5367bd4e404 | 251 | wait(0.5); |
jjmedina | 0:a5367bd4e404 | 252 | } |
jjmedina | 0:a5367bd4e404 | 253 | if(green_PUSH.read() == 1) |
jjmedina | 0:a5367bd4e404 | 254 | { |
jjmedina | 0:a5367bd4e404 | 255 | TRADUCE = 1;//traduce gui al ingles |
jjmedina | 0:a5367bd4e404 | 256 | SeleccionIdioma = 1; |
jjmedina | 0:a5367bd4e404 | 257 | wait(0.5); |
jjmedina | 0:a5367bd4e404 | 258 | } |
jjmedina | 0:a5367bd4e404 | 259 | } |
jjmedina | 0:a5367bd4e404 | 260 | |
jjmedina | 0:a5367bd4e404 | 261 | ///////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 262 | /// SELECCION DE LA FRECUENCIA DE ADQUISICION /// |
jjmedina | 0:a5367bd4e404 | 263 | ///////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 264 | |
jjmedina | 0:a5367bd4e404 | 265 | if(TRADUCE == 0) |
jjmedina | 0:a5367bd4e404 | 266 | { |
jjmedina | 0:a5367bd4e404 | 267 | display.Clear(); display.SetXY(0,0); display.printf("320 Hz: amarillo"); display.SetXY(0,1); display.printf("1000 Hz: rojo"); |
jjmedina | 0:a5367bd4e404 | 268 | } |
jjmedina | 0:a5367bd4e404 | 269 | if(TRADUCE == 1) |
jjmedina | 0:a5367bd4e404 | 270 | { |
jjmedina | 0:a5367bd4e404 | 271 | display.Clear(); display.SetXY(0,0); display.printf("320 Hz: yellow"); display.SetXY(0,1); display.printf("1000 Hz: red"); |
jjmedina | 0:a5367bd4e404 | 272 | } |
jjmedina | 0:a5367bd4e404 | 273 | |
jjmedina | 0:a5367bd4e404 | 274 | while(SeleccionFrecAdq == 0) |
jjmedina | 0:a5367bd4e404 | 275 | { |
jjmedina | 0:a5367bd4e404 | 276 | if(red_PUSH.read() == 1) |
jjmedina | 0:a5367bd4e404 | 277 | { |
jjmedina | 0:a5367bd4e404 | 278 | FrecAdq = 1000; |
jjmedina | 0:a5367bd4e404 | 279 | SeleccionFrecAdq = 1; |
jjmedina | 0:a5367bd4e404 | 280 | wait(0.5); |
jjmedina | 0:a5367bd4e404 | 281 | } |
jjmedina | 0:a5367bd4e404 | 282 | if(green_PUSH.read() == 1) |
jjmedina | 0:a5367bd4e404 | 283 | { |
jjmedina | 0:a5367bd4e404 | 284 | FrecAdq = 320; |
jjmedina | 0:a5367bd4e404 | 285 | SeleccionFrecAdq = 1; |
jjmedina | 0:a5367bd4e404 | 286 | wait(0.5); |
jjmedina | 0:a5367bd4e404 | 287 | } |
jjmedina | 0:a5367bd4e404 | 288 | } |
jjmedina | 0:a5367bd4e404 | 289 | |
jjmedina | 0:a5367bd4e404 | 290 | ParamDependFrecAdq(); //Establece el valor de los parámetros que depeneden de la frecuencia de adquisicion |
jjmedina | 0:a5367bd4e404 | 291 | |
jjmedina | 0:a5367bd4e404 | 292 | //////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 293 | /// AVISA MODO FUNCIONAMIENTO: PC O AUTONOMO /// |
jjmedina | 0:a5367bd4e404 | 294 | //////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 295 | |
jjmedina | 0:a5367bd4e404 | 296 | if(datosPC_green == true) |
jjmedina | 0:a5367bd4e404 | 297 | { |
jjmedina | 0:a5367bd4e404 | 298 | green_LED = 1; |
jjmedina | 0:a5367bd4e404 | 299 | if(TRADUCE == 0) |
jjmedina | 0:a5367bd4e404 | 300 | { |
jjmedina | 0:a5367bd4e404 | 301 | display.Clear(); display.SetXY(0,0); display.printf("Aparato en"); display.SetXY(0,1); display.printf("modo PC."); |
jjmedina | 0:a5367bd4e404 | 302 | wait(1.5); |
jjmedina | 0:a5367bd4e404 | 303 | display.Clear(); display.SetXY(0,0); display.printf("Para cambio a"); display.SetXY(0,1); display.printf("modo autonomo..."); |
jjmedina | 0:a5367bd4e404 | 304 | wait(1.5); |
jjmedina | 0:a5367bd4e404 | 305 | display.Clear(); display.SetXY(0,0); display.printf("...pulse boton"); display.SetXY(0,1); display.printf("amarillo."); |
jjmedina | 0:a5367bd4e404 | 306 | wait(1.5); |
jjmedina | 0:a5367bd4e404 | 307 | display.Clear(); |
jjmedina | 0:a5367bd4e404 | 308 | } |
jjmedina | 0:a5367bd4e404 | 309 | else |
jjmedina | 0:a5367bd4e404 | 310 | { |
jjmedina | 0:a5367bd4e404 | 311 | display.Clear(); display.SetXY(0,0); display.printf("Set in PC mode."); |
jjmedina | 0:a5367bd4e404 | 312 | wait(1.5); |
jjmedina | 0:a5367bd4e404 | 313 | display.Clear(); display.SetXY(0,0); display.printf("To change to"); display.SetXY(0,1); display.printf("autonomous mode"); |
jjmedina | 0:a5367bd4e404 | 314 | wait(1.5); |
jjmedina | 0:a5367bd4e404 | 315 | display.Clear(); display.SetXY(0,0); display.printf("...press"); display.SetXY(0,1); display.printf("yellow button."); |
jjmedina | 0:a5367bd4e404 | 316 | wait(1.5); |
jjmedina | 0:a5367bd4e404 | 317 | display.Clear(); |
jjmedina | 0:a5367bd4e404 | 318 | } |
jjmedina | 0:a5367bd4e404 | 319 | } |
jjmedina | 0:a5367bd4e404 | 320 | else |
jjmedina | 0:a5367bd4e404 | 321 | { |
jjmedina | 0:a5367bd4e404 | 322 | green_LED = 0; |
jjmedina | 0:a5367bd4e404 | 323 | |
jjmedina | 0:a5367bd4e404 | 324 | if(TRADUCE == 0) |
jjmedina | 0:a5367bd4e404 | 325 | { |
jjmedina | 0:a5367bd4e404 | 326 | display.Clear(); display.SetXY(0,0); display.printf("Aparato en"); display.SetXY(0,1); display.printf("modo autonomo."); |
jjmedina | 0:a5367bd4e404 | 327 | wait(1.5); |
jjmedina | 0:a5367bd4e404 | 328 | display.Clear(); display.SetXY(0,0); display.printf("Para cambio a"); display.SetXY(0,1); display.printf("modo PC..."); |
jjmedina | 0:a5367bd4e404 | 329 | wait(1.5); |
jjmedina | 0:a5367bd4e404 | 330 | display.Clear(); display.SetXY(0,0); display.printf("...pulse boton"); display.SetXY(0,1); display.printf("amarillo."); |
jjmedina | 0:a5367bd4e404 | 331 | wait(1.5); |
jjmedina | 0:a5367bd4e404 | 332 | display.Clear(); |
jjmedina | 0:a5367bd4e404 | 333 | } |
jjmedina | 0:a5367bd4e404 | 334 | else |
jjmedina | 0:a5367bd4e404 | 335 | { |
jjmedina | 0:a5367bd4e404 | 336 | display.Clear(); display.SetXY(0,0); display.printf("Set in "); display.SetXY(0,1); display.printf("autonomous mode."); |
jjmedina | 0:a5367bd4e404 | 337 | wait(1.5); |
jjmedina | 0:a5367bd4e404 | 338 | display.Clear(); display.SetXY(0,0); display.printf("To change to"); display.SetXY(0,1); display.printf("PC mode..."); |
jjmedina | 0:a5367bd4e404 | 339 | wait(1.5); |
jjmedina | 0:a5367bd4e404 | 340 | display.Clear(); display.SetXY(0,0); display.printf("...press"); display.SetXY(0,1); display.printf("yellow button."); |
jjmedina | 0:a5367bd4e404 | 341 | wait(1.5); |
jjmedina | 0:a5367bd4e404 | 342 | display.Clear(); |
jjmedina | 0:a5367bd4e404 | 343 | } |
jjmedina | 0:a5367bd4e404 | 344 | } |
jjmedina | 0:a5367bd4e404 | 345 | |
jjmedina | 0:a5367bd4e404 | 346 | |
jjmedina | 0:a5367bd4e404 | 347 | |
jjmedina | 0:a5367bd4e404 | 348 | while((SeleccionIdioma == 1)&&(SeleccionFrecAdq == 1)) |
jjmedina | 0:a5367bd4e404 | 349 | { |
jjmedina | 0:a5367bd4e404 | 350 | |
jjmedina | 0:a5367bd4e404 | 351 | //////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 352 | /// INVITA A COMENZAR LA ADQUISICION /// |
jjmedina | 0:a5367bd4e404 | 353 | //////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 354 | |
jjmedina | 0:a5367bd4e404 | 355 | if(ensayo_red == false) |
jjmedina | 0:a5367bd4e404 | 356 | { |
jjmedina | 0:a5367bd4e404 | 357 | if(TRADUCE == 0) |
jjmedina | 0:a5367bd4e404 | 358 | { |
jjmedina | 0:a5367bd4e404 | 359 | display.Clear(); |
jjmedina | 0:a5367bd4e404 | 360 | display.SetXY(0,0); display.printf("Pulse rojo"); |
jjmedina | 0:a5367bd4e404 | 361 | display.SetXY(0,1); display.printf("para empezar"); |
jjmedina | 0:a5367bd4e404 | 362 | wait(0.1); |
jjmedina | 0:a5367bd4e404 | 363 | } |
jjmedina | 0:a5367bd4e404 | 364 | if(TRADUCE == 1) |
jjmedina | 0:a5367bd4e404 | 365 | { |
jjmedina | 0:a5367bd4e404 | 366 | display.Clear(); |
jjmedina | 0:a5367bd4e404 | 367 | display.SetXY(0,0); display.printf("Push red"); |
jjmedina | 0:a5367bd4e404 | 368 | display.SetXY(0,1); display.printf("to start"); |
jjmedina | 0:a5367bd4e404 | 369 | wait(0.1); |
jjmedina | 0:a5367bd4e404 | 370 | } |
jjmedina | 0:a5367bd4e404 | 371 | } |
jjmedina | 0:a5367bd4e404 | 372 | /////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 373 | // COMIENZO Y FIN DE MEDIDDA // |
jjmedina | 0:a5367bd4e404 | 374 | /////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 375 | |
jjmedina | 0:a5367bd4e404 | 376 | if(red_PUSH.read() == 1) //si pulsa el rojo (o empieza o acaba medidas.) |
jjmedina | 0:a5367bd4e404 | 377 | { |
jjmedina | 0:a5367bd4e404 | 378 | if(ensayo_red == false)//antes de pulsar estaba apagado, por tanto empieza medidas. |
jjmedina | 0:a5367bd4e404 | 379 | { |
jjmedina | 0:a5367bd4e404 | 380 | ensayo_red = true;//lo enciende |
jjmedina | 0:a5367bd4e404 | 381 | |
jjmedina | 0:a5367bd4e404 | 382 | //////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 383 | /// ENCIENDE EL ACELEROMETRO /// |
jjmedina | 0:a5367bd4e404 | 384 | //////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 385 | |
jjmedina | 0:a5367bd4e404 | 386 | bma180.AcelerometroWakeUp(); |
jjmedina | 0:a5367bd4e404 | 387 | |
jjmedina | 0:a5367bd4e404 | 388 | 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) |
jjmedina | 0:a5367bd4e404 | 389 | { |
jjmedina | 0:a5367bd4e404 | 390 | // LEE DE FICHERO EL ORDEN DEL ENSAYO ANTERIOR Y VUELVE A GUARDAR EL ORDEN INCREMENTADO EN 1 |
jjmedina | 0:a5367bd4e404 | 391 | fp2 = fopen("/mbedFlash/OrdEns.txt","r"); |
jjmedina | 0:a5367bd4e404 | 392 | if(fp2 == NULL) |
jjmedina | 0:a5367bd4e404 | 393 | { |
jjmedina | 0:a5367bd4e404 | 394 | //SI NO EXISTE EL ARCHIVO CON EL ORDEN DEL ENSAYO SE CREA Y SE PONE UN 0 |
jjmedina | 0:a5367bd4e404 | 395 | pc.printf("Could not open file OrdEns.txt for read\n"); |
jjmedina | 0:a5367bd4e404 | 396 | fp2 = fopen("/mbedFlash/OrdEns.txt","w"); |
jjmedina | 0:a5367bd4e404 | 397 | if(fp2 == NULL) { pc.printf("Could not open file OrdEns.txt for write\n"); } |
jjmedina | 0:a5367bd4e404 | 398 | fprintf(fp2,"0"); |
jjmedina | 0:a5367bd4e404 | 399 | fclose(fp2); |
jjmedina | 0:a5367bd4e404 | 400 | } |
jjmedina | 0:a5367bd4e404 | 401 | fscanf(fp2,"%d", &ContadorEnsayos); |
jjmedina | 0:a5367bd4e404 | 402 | ContadorEnsayos++; |
jjmedina | 0:a5367bd4e404 | 403 | |
jjmedina | 0:a5367bd4e404 | 404 | if(TRADUCE==0) |
jjmedina | 0:a5367bd4e404 | 405 | { |
jjmedina | 0:a5367bd4e404 | 406 | display.Clear(); display.SetXY(0,0); display.printf("Comienza medida"); display.SetXY(0,1); display.printf("Viaje %d ...", ContadorEnsayos); |
jjmedina | 0:a5367bd4e404 | 407 | } |
jjmedina | 0:a5367bd4e404 | 408 | else |
jjmedina | 0:a5367bd4e404 | 409 | { |
jjmedina | 0:a5367bd4e404 | 410 | display.Clear(); display.SetXY(0,0); display.printf("Measuring ride"); display.SetXY(0,1); display.printf("number %d ...", ContadorEnsayos); |
jjmedina | 0:a5367bd4e404 | 411 | } |
jjmedina | 0:a5367bd4e404 | 412 | |
jjmedina | 0:a5367bd4e404 | 413 | |
jjmedina | 0:a5367bd4e404 | 414 | if(ContadorEnsayos == 100) { ContadorEnsayos = 0; } //EL NUMERO MAXIMO DE ENSAYOS ES 100. SI SE SUPERA SE PONE EL CONTADOR A 0 |
jjmedina | 0:a5367bd4e404 | 415 | pc.printf("Ensayo: %d\n",ContadorEnsayos); |
jjmedina | 0:a5367bd4e404 | 416 | fclose(fp2); |
jjmedina | 0:a5367bd4e404 | 417 | fp2 = fopen("/mbedFlash/OrdEns.txt","w"); |
jjmedina | 0:a5367bd4e404 | 418 | if(fp2 == NULL) { pc.printf("Could not open file OrdEns.txt for write\n"); } |
jjmedina | 0:a5367bd4e404 | 419 | fprintf(fp2,"%d", ContadorEnsayos); |
jjmedina | 0:a5367bd4e404 | 420 | fclose(fp2); |
jjmedina | 0:a5367bd4e404 | 421 | |
jjmedina | 0:a5367bd4e404 | 422 | ContadorDatos = 0; |
jjmedina | 0:a5367bd4e404 | 423 | ContadorMedidas = 0; |
jjmedina | 0:a5367bd4e404 | 424 | //Crea y abre un nuevo archivo para guardar aceleraciones |
jjmedina | 0:a5367bd4e404 | 425 | char resultsFile[50]; |
jjmedina | 0:a5367bd4e404 | 426 | |
jjmedina | 0:a5367bd4e404 | 427 | strcpy(resultsFile,"/local/raw_V_"); |
jjmedina | 0:a5367bd4e404 | 428 | |
jjmedina | 0:a5367bd4e404 | 429 | int n = sprintf (NoViaje, "%d", ContadorEnsayos); |
jjmedina | 0:a5367bd4e404 | 430 | strcat(resultsFile,NoViaje); |
jjmedina | 0:a5367bd4e404 | 431 | strcat(resultsFile,".txt"); |
jjmedina | 0:a5367bd4e404 | 432 | |
jjmedina | 0:a5367bd4e404 | 433 | msc.disk_initialize(); |
jjmedina | 0:a5367bd4e404 | 434 | |
jjmedina | 0:a5367bd4e404 | 435 | fp = fopen(resultsFile,"w"); |
jjmedina | 0:a5367bd4e404 | 436 | |
jjmedina | 0:a5367bd4e404 | 437 | if(fp == NULL) |
jjmedina | 0:a5367bd4e404 | 438 | { |
jjmedina | 0:a5367bd4e404 | 439 | if(TRADUCE==0) |
jjmedina | 0:a5367bd4e404 | 440 | { |
jjmedina | 0:a5367bd4e404 | 441 | display.Clear();display.SetXY(0,0); display.printf("USB desconectado");display.SetXY(0,1); display.printf("o lleno"); |
jjmedina | 0:a5367bd4e404 | 442 | } |
jjmedina | 0:a5367bd4e404 | 443 | else |
jjmedina | 0:a5367bd4e404 | 444 | { |
jjmedina | 0:a5367bd4e404 | 445 | display.Clear();display.SetXY(0,0); display.printf("USB disconnected");display.SetXY(0,1); display.printf("or full"); |
jjmedina | 0:a5367bd4e404 | 446 | } |
jjmedina | 0:a5367bd4e404 | 447 | /////////////////////////////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 448 | /// LO DE ABAJO ES PARA QUE NO SE QUEDE COLGADO SI HEMOS OLVIDADO CONECTAR EL PEN-DRIVE /// |
jjmedina | 0:a5367bd4e404 | 449 | /////////////////////////////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 450 | ensayo_red = false; |
jjmedina | 0:a5367bd4e404 | 451 | ContadorEnsayos--; |
jjmedina | 0:a5367bd4e404 | 452 | fp2 = fopen("/mbedFlash/OrdEns.txt","w"); |
jjmedina | 0:a5367bd4e404 | 453 | if(fp2 == NULL) { pc.printf("Could not open file OrdEns.txt for write\n"); } |
jjmedina | 0:a5367bd4e404 | 454 | fprintf(fp2,"%d", ContadorEnsayos); |
jjmedina | 0:a5367bd4e404 | 455 | fclose(fp2); |
jjmedina | 0:a5367bd4e404 | 456 | } |
jjmedina | 0:a5367bd4e404 | 457 | |
jjmedina | 0:a5367bd4e404 | 458 | ///////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 459 | /// OJO: COMIENZA EL TICKER DE LECTURA DE DATOS DEL ACELERÓMETRO /// |
jjmedina | 0:a5367bd4e404 | 460 | ///////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 461 | if(ensayo_red == true) |
jjmedina | 0:a5367bd4e404 | 462 | { |
jjmedina | 0:a5367bd4e404 | 463 | tickerLectura.attach_us(&LeeAcelerometroBuffer, PasoTiempo); |
jjmedina | 0:a5367bd4e404 | 464 | } |
jjmedina | 0:a5367bd4e404 | 465 | } |
jjmedina | 0:a5367bd4e404 | 466 | |
jjmedina | 0:a5367bd4e404 | 467 | wait(0.5); |
jjmedina | 0:a5367bd4e404 | 468 | } |
jjmedina | 0:a5367bd4e404 | 469 | else //antes de pulsar estaba encendido |
jjmedina | 0:a5367bd4e404 | 470 | { |
jjmedina | 0:a5367bd4e404 | 471 | ensayo_red = false; // lo apaga, es decir acaba medidas y procesa datos si en modo autonomo |
jjmedina | 0:a5367bd4e404 | 472 | if(ensayo_red == true) { red_LED = 1;} else { red_LED = 0;}//enciende o apaga el LED rojo |
jjmedina | 0:a5367bd4e404 | 473 | |
jjmedina | 0:a5367bd4e404 | 474 | pc.printf("-25717505\n"); //Señal para vmRideanalizer |
jjmedina | 0:a5367bd4e404 | 475 | |
jjmedina | 0:a5367bd4e404 | 476 | ///////////////////////////// |
jjmedina | 0:a5367bd4e404 | 477 | /// APAGA EL ACELEROMETRO /// |
jjmedina | 0:a5367bd4e404 | 478 | ///////////////////////////// |
jjmedina | 0:a5367bd4e404 | 479 | |
jjmedina | 0:a5367bd4e404 | 480 | bma180.AcelerometroSleep(); |
jjmedina | 0:a5367bd4e404 | 481 | |
jjmedina | 0:a5367bd4e404 | 482 | if(datosPC_green == false)//procesa datos si en modo autonomo. Cuando entra aquí, estan LEDs apagados + ensayo_red = false + datosPC_green == false |
jjmedina | 0:a5367bd4e404 | 483 | { |
jjmedina | 0:a5367bd4e404 | 484 | /////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 485 | // POSTPROCESO DE DATOS EN MODO AUTONOMO //Primera tanda |
jjmedina | 0:a5367bd4e404 | 486 | /////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 487 | |
jjmedina | 0:a5367bd4e404 | 488 | ///////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 489 | /// OJO: FINALIZA EL TICKER DE LECTURA DE DATOS DEL ACELERÓMETRO /// |
jjmedina | 0:a5367bd4e404 | 490 | ///////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 491 | |
jjmedina | 0:a5367bd4e404 | 492 | tickerLectura.detach(); |
jjmedina | 0:a5367bd4e404 | 493 | |
jjmedina | 0:a5367bd4e404 | 494 | fclose(fp);//cierra fichero con datos en el USB |
jjmedina | 0:a5367bd4e404 | 495 | |
jjmedina | 0:a5367bd4e404 | 496 | //if(TRADUCE==0) |
jjmedina | 0:a5367bd4e404 | 497 | //{ |
jjmedina | 0:a5367bd4e404 | 498 | // display.Clear(); |
jjmedina | 0:a5367bd4e404 | 499 | // display.SetXY(0,0); display.printf("Interpola datos"); |
jjmedina | 0:a5367bd4e404 | 500 | //}else |
jjmedina | 0:a5367bd4e404 | 501 | //{ |
jjmedina | 0:a5367bd4e404 | 502 | // display.Clear(); |
jjmedina | 0:a5367bd4e404 | 503 | // display.SetXY(0,0); display.printf("Interpolating data"); |
jjmedina | 0:a5367bd4e404 | 504 | // display.SetXY(0,1); display.printf("data"); |
jjmedina | 0:a5367bd4e404 | 505 | //} |
jjmedina | 0:a5367bd4e404 | 506 | //InterpolaMedidasPerdidas(ContadorEnsayos, ContadorDatos); |
jjmedina | 0:a5367bd4e404 | 507 | |
jjmedina | 0:a5367bd4e404 | 508 | if(TRADUCE==0) |
jjmedina | 0:a5367bd4e404 | 509 | { |
jjmedina | 0:a5367bd4e404 | 510 | display.Clear(); display.SetXY(0,0); display.printf("Rojo: posprocesa"); display.SetXY(0,1); display.printf("Amarillo: fin"); |
jjmedina | 0:a5367bd4e404 | 511 | }else |
jjmedina | 0:a5367bd4e404 | 512 | { |
jjmedina | 0:a5367bd4e404 | 513 | display.Clear(); display.SetXY(0,0); display.printf("Red: postprocess"); display.SetXY(0,1); display.printf("Yellow: finalize"); |
jjmedina | 0:a5367bd4e404 | 514 | } |
jjmedina | 0:a5367bd4e404 | 515 | |
jjmedina | 0:a5367bd4e404 | 516 | wait(0.5); |
jjmedina | 0:a5367bd4e404 | 517 | |
jjmedina | 0:a5367bd4e404 | 518 | while(1) |
jjmedina | 0:a5367bd4e404 | 519 | { |
jjmedina | 0:a5367bd4e404 | 520 | /////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 521 | // POSTPROCESO DE DATOS EN MODO AUTONOMO //Primera tanda |
jjmedina | 0:a5367bd4e404 | 522 | /////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 523 | //loop infinito hasta que pulsa el rojo otra vez para ver mas resultados o el verde para terminar y grabar resultados en fichero |
jjmedina | 0:a5367bd4e404 | 524 | if(red_PUSH.read() == 1) //si pulsa el rojo |
jjmedina | 0:a5367bd4e404 | 525 | { |
jjmedina | 0:a5367bd4e404 | 526 | //ponemos variables globales a zero |
jjmedina | 0:a5367bd4e404 | 527 | //aceleraciones medias |
jjmedina | 0:a5367bd4e404 | 528 | aZm0 = 0.0; |
jjmedina | 0:a5367bd4e404 | 529 | aYm0 = 0.0; |
jjmedina | 0:a5367bd4e404 | 530 | aXm0 = 0.0; |
jjmedina | 0:a5367bd4e404 | 531 | //veloc truco Dani |
jjmedina | 0:a5367bd4e404 | 532 | vZmFin = 0.0; |
jjmedina | 0:a5367bd4e404 | 533 | vNom = 0.0; |
jjmedina | 0:a5367bd4e404 | 534 | //maximos ISO |
jjmedina | 0:a5367bd4e404 | 535 | amax = 0.0; |
jjmedina | 0:a5367bd4e404 | 536 | dmax = 0.0; |
jjmedina | 0:a5367bd4e404 | 537 | Vmax = 0.0; |
jjmedina | 0:a5367bd4e404 | 538 | jerkmax = 0.0; |
jjmedina | 0:a5367bd4e404 | 539 | max_peaktopeak_car_ = 0.0; |
jjmedina | 0:a5367bd4e404 | 540 | max_peaktopeak_ncar_ = 0.0; |
jjmedina | 0:a5367bd4e404 | 541 | max_peaktopeak_X_ = 0.0; |
jjmedina | 0:a5367bd4e404 | 542 | max_peaktopeak_Y_ = 0.0; |
jjmedina | 0:a5367bd4e404 | 543 | Vmax1 = 0.0; |
jjmedina | 0:a5367bd4e404 | 544 | Vmax2 = 0.0; |
jjmedina | 0:a5367bd4e404 | 545 | |
jjmedina | 0:a5367bd4e404 | 546 | if(TRADUCE==0) |
jjmedina | 0:a5367bd4e404 | 547 | { |
jjmedina | 0:a5367bd4e404 | 548 | display.Clear(); display.SetXY(0,0); display.printf("Comienza el"); display.SetXY(0,1); display.printf("postproceso ..."); |
jjmedina | 0:a5367bd4e404 | 549 | } |
jjmedina | 0:a5367bd4e404 | 550 | else |
jjmedina | 0:a5367bd4e404 | 551 | { |
jjmedina | 0:a5367bd4e404 | 552 | display.Clear(); display.SetXY(0,0); display.printf("Starting"); display.SetXY(0,1); display.printf("postprocess ..."); |
jjmedina | 0:a5367bd4e404 | 553 | } |
jjmedina | 0:a5367bd4e404 | 554 | |
jjmedina | 0:a5367bd4e404 | 555 | //InterpolaMedidasPerdidas(ContadorEnsayos, ContadorDatos); |
jjmedina | 0:a5367bd4e404 | 556 | |
jjmedina | 0:a5367bd4e404 | 557 | FiltradoPeriodosIntegracion(ContadorEnsayos, ContadorDatos); |
jjmedina | 0:a5367bd4e404 | 558 | |
jjmedina | 0:a5367bd4e404 | 559 | //display.Clear(); display.SetXY(0,0); display.printf("Interpolacion,"); display.SetXY(0,1); display.printf("filtro, integracion"); |
jjmedina | 0:a5367bd4e404 | 560 | |
jjmedina | 0:a5367bd4e404 | 561 | DatosConfort(ContadorEnsayos, ContadorDatos, PasoTiempo); |
jjmedina | 0:a5367bd4e404 | 562 | |
jjmedina | 0:a5367bd4e404 | 563 | //display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("Vel. nom. y jerk"); |
jjmedina | 0:a5367bd4e404 | 564 | |
jjmedina | 0:a5367bd4e404 | 565 | calcula_n05n95_Boveloc_jerkmax(ContadorEnsayos, ContadorDatos, PasoTiempo); |
jjmedina | 0:a5367bd4e404 | 566 | |
jjmedina | 0:a5367bd4e404 | 567 | //ha terminado de calcular amax, dmax, Vmax y jerkmax |
jjmedina | 0:a5367bd4e404 | 568 | |
jjmedina | 0:a5367bd4e404 | 569 | //display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("A95s"); |
jjmedina | 0:a5367bd4e404 | 570 | |
jjmedina | 0:a5367bd4e404 | 571 | calcula_A95_V_acel_decel(ContadorEnsayos, ContadorDatos, PasoTiempo); |
jjmedina | 0:a5367bd4e404 | 572 | |
jjmedina | 0:a5367bd4e404 | 573 | //ha terminado de calcular A95 de a, d y V. Muestra por pantalla |
jjmedina | 0:a5367bd4e404 | 574 | |
jjmedina | 0:a5367bd4e404 | 575 | DisplayResultados1(); |
jjmedina | 0:a5367bd4e404 | 576 | |
jjmedina | 0:a5367bd4e404 | 577 | |
jjmedina | 0:a5367bd4e404 | 578 | if(FrecAdq == 320) |
jjmedina | 0:a5367bd4e404 | 579 | { |
jjmedina | 0:a5367bd4e404 | 580 | if(TRADUCE==0) |
jjmedina | 0:a5367bd4e404 | 581 | { |
jjmedina | 0:a5367bd4e404 | 582 | display.Clear(); display.SetXY(0,0); display.printf("Rojo continua"); display.SetXY(0,1); display.printf("Amarillo fin"); |
jjmedina | 0:a5367bd4e404 | 583 | }else |
jjmedina | 0:a5367bd4e404 | 584 | { |
jjmedina | 0:a5367bd4e404 | 585 | display.Clear(); display.SetXY(0,0); display.printf("Red continue"); display.SetXY(0,1); display.printf("Yellow finalize"); |
jjmedina | 0:a5367bd4e404 | 586 | } |
jjmedina | 0:a5367bd4e404 | 587 | } |
jjmedina | 0:a5367bd4e404 | 588 | break;//ha calculado resultados restantes y finaliza |
jjmedina | 0:a5367bd4e404 | 589 | }else if(green_PUSH.read() == 1) |
jjmedina | 0:a5367bd4e404 | 590 | { |
jjmedina | 0:a5367bd4e404 | 591 | break; |
jjmedina | 0:a5367bd4e404 | 592 | } |
jjmedina | 0:a5367bd4e404 | 593 | }//primera tanda resultados |
jjmedina | 0:a5367bd4e404 | 594 | |
jjmedina | 0:a5367bd4e404 | 595 | |
jjmedina | 0:a5367bd4e404 | 596 | |
jjmedina | 0:a5367bd4e404 | 597 | while(FrecAdq == 320) |
jjmedina | 0:a5367bd4e404 | 598 | { |
jjmedina | 0:a5367bd4e404 | 599 | /////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 600 | // POSTPROCESO DE DATOS EN MODO AUTONOMO //Segunda tanda |
jjmedina | 0:a5367bd4e404 | 601 | /////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 602 | |
jjmedina | 0:a5367bd4e404 | 603 | //loop infinito hasta que pulsa el rojo otra vez para ver mas resultados o el verde para terminar y grabar resultados en fichero |
jjmedina | 0:a5367bd4e404 | 604 | |
jjmedina | 0:a5367bd4e404 | 605 | if(red_PUSH.read() == 1) //si pulsa el rojo |
jjmedina | 0:a5367bd4e404 | 606 | { |
jjmedina | 0:a5367bd4e404 | 607 | if(TRADUCE==0) |
jjmedina | 0:a5367bd4e404 | 608 | { |
jjmedina | 0:a5367bd4e404 | 609 | display.Clear(); display.SetXY(0,0); display.printf("Cont. Posproceso"); display.SetXY(0,1); display.printf("vibracion Z,X,Y"); |
jjmedina | 0:a5367bd4e404 | 610 | }else |
jjmedina | 0:a5367bd4e404 | 611 | { |
jjmedina | 0:a5367bd4e404 | 612 | display.Clear(); display.SetXY(0,0); display.printf("Con. Postprocess"); display.SetXY(0,1); display.printf("vibration Z,X,Y"); |
jjmedina | 0:a5367bd4e404 | 613 | } |
jjmedina | 0:a5367bd4e404 | 614 | //empieza cálculos de ZXYvibweighted |
jjmedina | 0:a5367bd4e404 | 615 | vibraReadFilter(ContadorEnsayos, ContadorDatos); |
jjmedina | 0:a5367bd4e404 | 616 | |
jjmedina | 0:a5367bd4e404 | 617 | //display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("step2"); |
jjmedina | 0:a5367bd4e404 | 618 | |
jjmedina | 0:a5367bd4e404 | 619 | calculaStep2( ContadorEnsayos, ContadorDatos); |
jjmedina | 0:a5367bd4e404 | 620 | |
jjmedina | 0:a5367bd4e404 | 621 | //display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("car-ncar"); |
jjmedina | 0:a5367bd4e404 | 622 | |
jjmedina | 0:a5367bd4e404 | 623 | calculaCarNCar( ContadorEnsayos, ContadorDatos, PasoTiempo ); //pc.printf("Termina 'calculaCarNCar'\n"); |
jjmedina | 0:a5367bd4e404 | 624 | |
jjmedina | 0:a5367bd4e404 | 625 | //display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("maxZXY"); |
jjmedina | 0:a5367bd4e404 | 626 | |
jjmedina | 0:a5367bd4e404 | 627 | calculaZXYvibweighted( ContadorEnsayos, ContadorDatos ); |
jjmedina | 0:a5367bd4e404 | 628 | |
jjmedina | 0:a5367bd4e404 | 629 | //display.Clear(); display.SetXY(0,0); display.printf("Calculando"); display.SetXY(0,1); display.printf("Display"); |
jjmedina | 0:a5367bd4e404 | 630 | |
jjmedina | 0:a5367bd4e404 | 631 | DisplayResultados2(); |
jjmedina | 0:a5367bd4e404 | 632 | |
jjmedina | 0:a5367bd4e404 | 633 | break;//ha calculado resultados restantes y finaliza |
jjmedina | 0:a5367bd4e404 | 634 | }else if(green_PUSH.read() == 1) |
jjmedina | 0:a5367bd4e404 | 635 | { |
jjmedina | 0:a5367bd4e404 | 636 | break; |
jjmedina | 0:a5367bd4e404 | 637 | } |
jjmedina | 0:a5367bd4e404 | 638 | } |
jjmedina | 0:a5367bd4e404 | 639 | |
jjmedina | 0:a5367bd4e404 | 640 | //segunda tanda resultados |
jjmedina | 0:a5367bd4e404 | 641 | //copia resultados en un fichero y finaliza |
jjmedina | 0:a5367bd4e404 | 642 | if(TRADUCE==0) |
jjmedina | 0:a5367bd4e404 | 643 | { |
jjmedina | 0:a5367bd4e404 | 644 | display.Clear(); display.SetXY(0,0); display.printf("Guarda resultados"); display.SetXY(0,1); display.printf("en fichero"); |
jjmedina | 0:a5367bd4e404 | 645 | }else |
jjmedina | 0:a5367bd4e404 | 646 | { |
jjmedina | 0:a5367bd4e404 | 647 | display.Clear(); display.SetXY(0,0); display.printf("Save results"); display.SetXY(0,1); display.printf("in file"); |
jjmedina | 0:a5367bd4e404 | 648 | } |
jjmedina | 0:a5367bd4e404 | 649 | |
jjmedina | 0:a5367bd4e404 | 650 | FileResultados( ContadorEnsayos ); |
jjmedina | 0:a5367bd4e404 | 651 | |
jjmedina | 0:a5367bd4e404 | 652 | wait(0.5); |
jjmedina | 0:a5367bd4e404 | 653 | |
jjmedina | 0:a5367bd4e404 | 654 | if(TRADUCE==0) |
jjmedina | 0:a5367bd4e404 | 655 | { |
jjmedina | 0:a5367bd4e404 | 656 | display.Clear(); display.SetXY(0,0); display.printf("Postproceso"); display.SetXY(0,1); display.printf("fin"); |
jjmedina | 0:a5367bd4e404 | 657 | }else |
jjmedina | 0:a5367bd4e404 | 658 | { |
jjmedina | 0:a5367bd4e404 | 659 | display.Clear(); display.SetXY(0,0); display.printf("Postprocess"); display.SetXY(0,1); display.printf("end"); |
jjmedina | 0:a5367bd4e404 | 660 | } |
jjmedina | 0:a5367bd4e404 | 661 | } |
jjmedina | 0:a5367bd4e404 | 662 | wait(0.5); |
jjmedina | 0:a5367bd4e404 | 663 | display.Clear(); |
jjmedina | 0:a5367bd4e404 | 664 | } |
jjmedina | 0:a5367bd4e404 | 665 | } |
jjmedina | 0:a5367bd4e404 | 666 | |
jjmedina | 0:a5367bd4e404 | 667 | if(ensayo_red == true) { red_LED = 1;} else { red_LED = 0;}//enciende o apaga el LED rojo |
jjmedina | 0:a5367bd4e404 | 668 | |
jjmedina | 0:a5367bd4e404 | 669 | ///////////////////////////// |
jjmedina | 0:a5367bd4e404 | 670 | ///MODOS DE FUNCIONAMIENTO:// |
jjmedina | 0:a5367bd4e404 | 671 | // AUTONOMO O PC //////////// |
jjmedina | 0:a5367bd4e404 | 672 | ///////////////////////////// |
jjmedina | 0:a5367bd4e404 | 673 | |
jjmedina | 0:a5367bd4e404 | 674 | if(ensayo_red == false) |
jjmedina | 0:a5367bd4e404 | 675 | { |
jjmedina | 0:a5367bd4e404 | 676 | if(green_PUSH.read() == 1) |
jjmedina | 0:a5367bd4e404 | 677 | { |
jjmedina | 0:a5367bd4e404 | 678 | datosPC_green = !datosPC_green; |
jjmedina | 0:a5367bd4e404 | 679 | |
jjmedina | 0:a5367bd4e404 | 680 | if(datosPC_green == true) |
jjmedina | 0:a5367bd4e404 | 681 | { |
jjmedina | 0:a5367bd4e404 | 682 | if(TRADUCE == 0) |
jjmedina | 0:a5367bd4e404 | 683 | { |
jjmedina | 0:a5367bd4e404 | 684 | display.Clear(); display.SetXY(0,0); display.printf("Aparato en"); display.SetXY(0,1); display.printf("modo PC."); |
jjmedina | 0:a5367bd4e404 | 685 | wait(1.5); |
jjmedina | 0:a5367bd4e404 | 686 | display.Clear(); |
jjmedina | 0:a5367bd4e404 | 687 | } |
jjmedina | 0:a5367bd4e404 | 688 | else |
jjmedina | 0:a5367bd4e404 | 689 | { |
jjmedina | 0:a5367bd4e404 | 690 | display.Clear(); display.SetXY(0,0); display.printf("Set in PC mode."); |
jjmedina | 0:a5367bd4e404 | 691 | wait(1.5); |
jjmedina | 0:a5367bd4e404 | 692 | display.Clear(); |
jjmedina | 0:a5367bd4e404 | 693 | } |
jjmedina | 0:a5367bd4e404 | 694 | } |
jjmedina | 0:a5367bd4e404 | 695 | |
jjmedina | 0:a5367bd4e404 | 696 | if(datosPC_green == false) |
jjmedina | 0:a5367bd4e404 | 697 | { |
jjmedina | 0:a5367bd4e404 | 698 | if(TRADUCE == 0) |
jjmedina | 0:a5367bd4e404 | 699 | { |
jjmedina | 0:a5367bd4e404 | 700 | display.Clear(); display.SetXY(0,0); display.printf("Aparato en"); display.SetXY(0,1); display.printf("modo autonomo."); |
jjmedina | 0:a5367bd4e404 | 701 | wait(1.5); |
jjmedina | 0:a5367bd4e404 | 702 | display.Clear(); |
jjmedina | 0:a5367bd4e404 | 703 | } |
jjmedina | 0:a5367bd4e404 | 704 | else |
jjmedina | 0:a5367bd4e404 | 705 | { |
jjmedina | 0:a5367bd4e404 | 706 | display.Clear(); display.SetXY(0,0); display.printf("Set in"); display.SetXY(0,1); display.printf("autonomous mode."); |
jjmedina | 0:a5367bd4e404 | 707 | wait(1.5); |
jjmedina | 0:a5367bd4e404 | 708 | display.Clear(); |
jjmedina | 0:a5367bd4e404 | 709 | } |
jjmedina | 0:a5367bd4e404 | 710 | } |
jjmedina | 0:a5367bd4e404 | 711 | } |
jjmedina | 0:a5367bd4e404 | 712 | |
jjmedina | 0:a5367bd4e404 | 713 | if(datosPC_green == true) |
jjmedina | 0:a5367bd4e404 | 714 | { |
jjmedina | 0:a5367bd4e404 | 715 | green_LED = 1; |
jjmedina | 0:a5367bd4e404 | 716 | } |
jjmedina | 0:a5367bd4e404 | 717 | else |
jjmedina | 0:a5367bd4e404 | 718 | { |
jjmedina | 0:a5367bd4e404 | 719 | green_LED = 0; |
jjmedina | 0:a5367bd4e404 | 720 | }//enciende o apaga LED verde |
jjmedina | 0:a5367bd4e404 | 721 | } |
jjmedina | 0:a5367bd4e404 | 722 | |
jjmedina | 0:a5367bd4e404 | 723 | ///////////////////////////// |
jjmedina | 0:a5367bd4e404 | 724 | // MEDIDA DE ACELERACIONES // |
jjmedina | 0:a5367bd4e404 | 725 | ///////////////////////////// |
jjmedina | 0:a5367bd4e404 | 726 | |
jjmedina | 0:a5367bd4e404 | 727 | if(ensayo_red == true) |
jjmedina | 0:a5367bd4e404 | 728 | { |
jjmedina | 0:a5367bd4e404 | 729 | |
jjmedina | 0:a5367bd4e404 | 730 | if(datosPC_green == true)//Manda los datos al PC, puerto serie |
jjmedina | 0:a5367bd4e404 | 731 | { |
jjmedina | 0:a5367bd4e404 | 732 | //////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 733 | /// LECTURA CON ACELEROMETRO BMA180, DIGITAL /// |
jjmedina | 0:a5367bd4e404 | 734 | //////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 735 | |
jjmedina | 0:a5367bd4e404 | 736 | bma180.ReadAccels_BMA180(readings); |
jjmedina | 0:a5367bd4e404 | 737 | |
jjmedina | 0:a5367bd4e404 | 738 | int16_t aaX = (int16_t)readings[0]; |
jjmedina | 0:a5367bd4e404 | 739 | int16_t aaY = (int16_t)readings[1]; |
jjmedina | 0:a5367bd4e404 | 740 | int16_t aaZ = (int16_t)readings[2]; |
jjmedina | 0:a5367bd4e404 | 741 | |
jjmedina | 0:a5367bd4e404 | 742 | aX = (-1.0)*((float)aaX-(-103.23))*0.0018; |
jjmedina | 0:a5367bd4e404 | 743 | aY = (-1.0)*((float)aaY-(57.21))*0.0018; |
jjmedina | 0:a5367bd4e404 | 744 | aZ = (-1.0)*((float)aaZ-(701.1184))*0.0018097570403; |
jjmedina | 0:a5367bd4e404 | 745 | |
jjmedina | 0:a5367bd4e404 | 746 | ContadorMedidas++; |
jjmedina | 0:a5367bd4e404 | 747 | |
jjmedina | 0:a5367bd4e404 | 748 | pc.printf("%d,%f,%f,%f\n", timer.read_us(), aX, aY, aZ); |
jjmedina | 0:a5367bd4e404 | 749 | } |
jjmedina | 0:a5367bd4e404 | 750 | |
jjmedina | 0:a5367bd4e404 | 751 | |
jjmedina | 0:a5367bd4e404 | 752 | if(datosPC_green == false)//Manda los datos a fichero USB y promedia medidas perdidas |
jjmedina | 0:a5367bd4e404 | 753 | { |
jjmedina | 0:a5367bd4e404 | 754 | |
jjmedina | 0:a5367bd4e404 | 755 | GuardaDatosBuffer(); |
jjmedina | 0:a5367bd4e404 | 756 | |
jjmedina | 0:a5367bd4e404 | 757 | /* |
jjmedina | 0:a5367bd4e404 | 758 | //////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 759 | // PROMEDIO MEDIDAS ACELERACIONES // |
jjmedina | 0:a5367bd4e404 | 760 | //////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 761 | |
jjmedina | 0:a5367bd4e404 | 762 | if(ContadorMedidas == 1) {TiempoInicio = timer.read_us();} |
jjmedina | 0:a5367bd4e404 | 763 | |
jjmedina | 0:a5367bd4e404 | 764 | TiempoFin = timer.read_us(); |
jjmedina | 0:a5367bd4e404 | 765 | |
jjmedina | 0:a5367bd4e404 | 766 | if((TiempoFin-TiempoInicio)<((ContadorDatos+1)*PasoTiempo)) |
jjmedina | 0:a5367bd4e404 | 767 | { |
jjmedina | 0:a5367bd4e404 | 768 | nDatosPorPaso++; |
jjmedina | 0:a5367bd4e404 | 769 | aXm += aX; aYm += aY; aZm += aZ; |
jjmedina | 0:a5367bd4e404 | 770 | } |
jjmedina | 0:a5367bd4e404 | 771 | else |
jjmedina | 0:a5367bd4e404 | 772 | { |
jjmedina | 0:a5367bd4e404 | 773 | //Calculo del numero de pasos en blanco |
jjmedina | 0:a5367bd4e404 | 774 | int PasosEnBlanco = ((TiempoFin-TiempoInicio)-(ContadorDatos+1)*PasoTiempo)/(PasoTiempo); |
jjmedina | 0:a5367bd4e404 | 775 | |
jjmedina | 0:a5367bd4e404 | 776 | if(PasosEnBlanco == 0) |
jjmedina | 0:a5367bd4e404 | 777 | { |
jjmedina | 0:a5367bd4e404 | 778 | fprintf(fp,"%d %f %f %f %d\n", ContadorDatos*PasoTiempo, aXm/(float)nDatosPorPaso, aYm/(float)nDatosPorPaso, aZm/(float)nDatosPorPaso, nDatosPorPaso); |
jjmedina | 0:a5367bd4e404 | 779 | aXm = aX; aYm = aY; aZm = aZ; |
jjmedina | 0:a5367bd4e404 | 780 | nDatosPorPaso = 1; |
jjmedina | 0:a5367bd4e404 | 781 | ContadorDatos ++; |
jjmedina | 0:a5367bd4e404 | 782 | } |
jjmedina | 0:a5367bd4e404 | 783 | else |
jjmedina | 0:a5367bd4e404 | 784 | { |
jjmedina | 0:a5367bd4e404 | 785 | for(int i = 0; i<PasosEnBlanco; i++) |
jjmedina | 0:a5367bd4e404 | 786 | { |
jjmedina | 0:a5367bd4e404 | 787 | fprintf(fp,"%d %f %f %f %d\n", (ContadorDatos+i)*PasoTiempo, -20.0, -20.0, -20.0, 0); |
jjmedina | 0:a5367bd4e404 | 788 | //fprintf(fp,"t = %d, PasosEnblanco = %d, FinalMedida = %d \n", (ContadorDatos+i)*PasoTiempo, PasosEnBlanco,TiempoFin-TiempoInicio); |
jjmedina | 0:a5367bd4e404 | 789 | |
jjmedina | 0:a5367bd4e404 | 790 | aXm = aX; aYm = aY; aZm = aZ; |
jjmedina | 0:a5367bd4e404 | 791 | } |
jjmedina | 0:a5367bd4e404 | 792 | nDatosPorPaso = 1; |
jjmedina | 0:a5367bd4e404 | 793 | ContadorDatos = ContadorDatos + PasosEnBlanco; |
jjmedina | 0:a5367bd4e404 | 794 | } |
jjmedina | 0:a5367bd4e404 | 795 | }*/ |
jjmedina | 0:a5367bd4e404 | 796 | } |
jjmedina | 0:a5367bd4e404 | 797 | } |
jjmedina | 0:a5367bd4e404 | 798 | } |
jjmedina | 0:a5367bd4e404 | 799 | } |
jjmedina | 0:a5367bd4e404 | 800 | |
jjmedina | 0:a5367bd4e404 | 801 | |
jjmedina | 0:a5367bd4e404 | 802 | void InterpolaMedidasPerdidas(unsigned int ContadorEnsayos, unsigned int ContadorDatos) |
jjmedina | 0:a5367bd4e404 | 803 | { |
jjmedina | 0:a5367bd4e404 | 804 | int t; |
jjmedina | 0:a5367bd4e404 | 805 | float aXYZ[3]; |
jjmedina | 0:a5367bd4e404 | 806 | float aXYZantes[3]; |
jjmedina | 0:a5367bd4e404 | 807 | float aXYZdespues[3]; |
jjmedina | 0:a5367bd4e404 | 808 | float aXYZinterpolada[3]; |
jjmedina | 0:a5367bd4e404 | 809 | int tPasosPerdidos[300]; |
jjmedina | 0:a5367bd4e404 | 810 | |
jjmedina | 0:a5367bd4e404 | 811 | FILE * fp; |
jjmedina | 0:a5367bd4e404 | 812 | FILE * fp2; |
jjmedina | 0:a5367bd4e404 | 813 | |
jjmedina | 0:a5367bd4e404 | 814 | char NoViajel[2]; |
jjmedina | 0:a5367bd4e404 | 815 | char resultsFile[50]; char interpolatedFile[50]; |
jjmedina | 0:a5367bd4e404 | 816 | |
jjmedina | 0:a5367bd4e404 | 817 | strcpy(resultsFile,"/local/raw_V_"); strcpy(interpolatedFile,"/local/IV_"); |
jjmedina | 0:a5367bd4e404 | 818 | |
jjmedina | 0:a5367bd4e404 | 819 | sprintf (NoViajel, "%d", ContadorEnsayos); |
jjmedina | 0:a5367bd4e404 | 820 | strcat(resultsFile,NoViajel); strcat(interpolatedFile,NoViajel); |
jjmedina | 0:a5367bd4e404 | 821 | strcat(resultsFile,".txt"); strcat(interpolatedFile,".dat"); |
jjmedina | 0:a5367bd4e404 | 822 | |
jjmedina | 0:a5367bd4e404 | 823 | fp = fopen(resultsFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", resultsFile); } |
jjmedina | 0:a5367bd4e404 | 824 | fp2 = fopen(interpolatedFile,"w"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", interpolatedFile); } |
jjmedina | 0:a5367bd4e404 | 825 | |
jjmedina | 0:a5367bd4e404 | 826 | unsigned int i = 0; |
jjmedina | 0:a5367bd4e404 | 827 | unsigned int nDatosPorPaso; |
jjmedina | 0:a5367bd4e404 | 828 | unsigned int PasosPerdidos = 0; |
jjmedina | 0:a5367bd4e404 | 829 | |
jjmedina | 0:a5367bd4e404 | 830 | while(i<ContadorDatos) |
jjmedina | 0:a5367bd4e404 | 831 | { |
jjmedina | 0:a5367bd4e404 | 832 | fscanf(fp,"%d %f %f %f %d", &t, &aXYZ[0], &aXYZ[1], &aXYZ[2], &nDatosPorPaso); |
jjmedina | 0:a5367bd4e404 | 833 | |
jjmedina | 0:a5367bd4e404 | 834 | |
jjmedina | 0:a5367bd4e404 | 835 | |
jjmedina | 0:a5367bd4e404 | 836 | if(nDatosPorPaso >0) |
jjmedina | 0:a5367bd4e404 | 837 | { |
jjmedina | 0:a5367bd4e404 | 838 | aXYZantes[0] = aXYZ[0]; aXYZantes[1] = aXYZ[1]; aXYZantes[2] = aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 839 | fprintf(fp2,"%f %f %f %f\n", (float)t/1000000.0, aXYZ[0], aXYZ[1], aXYZ[2]); |
jjmedina | 0:a5367bd4e404 | 840 | i++; |
jjmedina | 0:a5367bd4e404 | 841 | } |
jjmedina | 0:a5367bd4e404 | 842 | else |
jjmedina | 0:a5367bd4e404 | 843 | { |
jjmedina | 0:a5367bd4e404 | 844 | tPasosPerdidos[PasosPerdidos] = t; |
jjmedina | 0:a5367bd4e404 | 845 | PasosPerdidos = 1; |
jjmedina | 0:a5367bd4e404 | 846 | |
jjmedina | 0:a5367bd4e404 | 847 | while(nDatosPorPaso == 0) |
jjmedina | 0:a5367bd4e404 | 848 | { |
jjmedina | 0:a5367bd4e404 | 849 | fscanf(fp,"%d %f %f %f %d", &t, &aXYZ[0], &aXYZ[1], &aXYZ[2], &nDatosPorPaso); |
jjmedina | 0:a5367bd4e404 | 850 | |
jjmedina | 0:a5367bd4e404 | 851 | if(nDatosPorPaso == 0) |
jjmedina | 0:a5367bd4e404 | 852 | { |
jjmedina | 0:a5367bd4e404 | 853 | tPasosPerdidos[PasosPerdidos] = t; |
jjmedina | 0:a5367bd4e404 | 854 | PasosPerdidos++; |
jjmedina | 0:a5367bd4e404 | 855 | } |
jjmedina | 0:a5367bd4e404 | 856 | else |
jjmedina | 0:a5367bd4e404 | 857 | { |
jjmedina | 0:a5367bd4e404 | 858 | aXYZdespues[0] = aXYZ[0]; aXYZdespues[1] = aXYZ[1]; aXYZdespues[2] = aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 859 | } |
jjmedina | 0:a5367bd4e404 | 860 | } |
jjmedina | 0:a5367bd4e404 | 861 | |
jjmedina | 0:a5367bd4e404 | 862 | for(unsigned int j = 0; j<PasosPerdidos; j++) |
jjmedina | 0:a5367bd4e404 | 863 | { |
jjmedina | 0:a5367bd4e404 | 864 | //aXYZinterpolada[0] = aXYZantes[0]+((aXYZdespues[0]-aXYZantes[0])/(float(PasosPerdidos+1)))*(float)(j+1); |
jjmedina | 0:a5367bd4e404 | 865 | //aXYZinterpolada[1] = aXYZantes[1]+((aXYZdespues[1]-aXYZantes[1])/(float(PasosPerdidos+1)))*(float)(j+1); |
jjmedina | 0:a5367bd4e404 | 866 | //aXYZinterpolada[2] = aXYZantes[2]+((aXYZdespues[2]-aXYZantes[2])/(float(PasosPerdidos+1)))*(float)(j+1); |
jjmedina | 0:a5367bd4e404 | 867 | |
jjmedina | 0:a5367bd4e404 | 868 | aXYZinterpolada[0] = (aXYZdespues[0]+aXYZantes[0])/2.0; |
jjmedina | 0:a5367bd4e404 | 869 | aXYZinterpolada[1] = (aXYZdespues[1]+aXYZantes[1])/2.0; |
jjmedina | 0:a5367bd4e404 | 870 | aXYZinterpolada[2] = (aXYZdespues[2]+aXYZantes[2])/2.0; |
jjmedina | 0:a5367bd4e404 | 871 | |
jjmedina | 0:a5367bd4e404 | 872 | fprintf(fp2,"%f %f %f %f\n", (float)tPasosPerdidos[j]/1000000.0, aXYZinterpolada[0], aXYZinterpolada[1], aXYZinterpolada[2]); |
jjmedina | 0:a5367bd4e404 | 873 | i++; |
jjmedina | 0:a5367bd4e404 | 874 | } |
jjmedina | 0:a5367bd4e404 | 875 | fprintf(fp2,"%f %f %f %f\n", (float)t/1000000.0, aXYZ[0], aXYZ[1], aXYZ[2]); |
jjmedina | 0:a5367bd4e404 | 876 | i++; |
jjmedina | 0:a5367bd4e404 | 877 | PasosPerdidos = 0; |
jjmedina | 0:a5367bd4e404 | 878 | } |
jjmedina | 0:a5367bd4e404 | 879 | } |
jjmedina | 0:a5367bd4e404 | 880 | |
jjmedina | 0:a5367bd4e404 | 881 | fclose(fp); |
jjmedina | 0:a5367bd4e404 | 882 | fclose(fp2); |
jjmedina | 0:a5367bd4e404 | 883 | |
jjmedina | 0:a5367bd4e404 | 884 | //elimina fichero RAW |
jjmedina | 0:a5367bd4e404 | 885 | int rm = remove(resultsFile); |
jjmedina | 0:a5367bd4e404 | 886 | } |
jjmedina | 0:a5367bd4e404 | 887 | |
jjmedina | 0:a5367bd4e404 | 888 | |
jjmedina | 0:a5367bd4e404 | 889 | void FiltradoPeriodosIntegracion(unsigned int ContadorEnsayos, unsigned int ContadorDatos) |
jjmedina | 0:a5367bd4e404 | 890 | { |
jjmedina | 0:a5367bd4e404 | 891 | float t; |
jjmedina | 0:a5367bd4e404 | 892 | float aXYZ[3]; //vector aceleracion en el paso i [aX,aY,aZ] |
jjmedina | 0:a5367bd4e404 | 893 | |
jjmedina | 0:a5367bd4e404 | 894 | float xv[3]; //Para hacer las operaciones del Filtro Butterworth |
jjmedina | 0:a5367bd4e404 | 895 | float yv[3]; //Para hacer las operaciones del Filtro Butterworth |
jjmedina | 0:a5367bd4e404 | 896 | |
jjmedina | 0:a5367bd4e404 | 897 | const float tol = 0.05; //Porcentaje en que la aceleracion debe superar la media para considerar que cambia el periodo del viaje |
jjmedina | 0:a5367bd4e404 | 898 | |
jjmedina | 0:a5367bd4e404 | 899 | bool mov = false; //Indica si hay o no movimiento |
jjmedina | 0:a5367bd4e404 | 900 | bool velV = false; //Indica si estamos en periodo de velocidad constante |
jjmedina | 0:a5367bd4e404 | 901 | bool decel = false; //Indica si ha empezado la deceleracion |
jjmedina | 0:a5367bd4e404 | 902 | bool fin = false; //Indica si ha terminado el viaje |
jjmedina | 0:a5367bd4e404 | 903 | //float t_Arr, t_V0, t_Vfin, t_Par; //Instantes en que empiezan arranque, ... |
jjmedina | 0:a5367bd4e404 | 904 | |
jjmedina | 0:a5367bd4e404 | 905 | float xZ = 0.0; //Posici�n vertical calculada integrando vZ |
jjmedina | 0:a5367bd4e404 | 906 | float vZ = 0.0; //Velocidad vertical calculada integrando la aceleraciOn filtrada menos su media |
jjmedina | 0:a5367bd4e404 | 907 | |
jjmedina | 0:a5367bd4e404 | 908 | float t_3[3], aZ_3[3], vZ_3[3], xZ_3[3]; //Datos guardados para hacer integracion numerica de aceleraciones |
jjmedina | 0:a5367bd4e404 | 909 | float xa, xb, xc, h, fa, fb, fc, fab, fbc, fabc; |
jjmedina | 0:a5367bd4e404 | 910 | |
jjmedina | 0:a5367bd4e404 | 911 | |
jjmedina | 0:a5367bd4e404 | 912 | unsigned int nmedV = 50; //Puntos para calcular la media de vZ al final del viaje (que debe ser zero pero no lo es) |
jjmedina | 0:a5367bd4e404 | 913 | |
jjmedina | 0:a5367bd4e404 | 914 | float sgnAcelArr = 0.0; //Signo de la aceleracion vertical de arranque |
jjmedina | 0:a5367bd4e404 | 915 | float sgnAcelFr = 0.0; //Signo de la aceleracion vertical de frenada |
jjmedina | 0:a5367bd4e404 | 916 | float sgnAcelv0 = 0.0; //Signo de la aceleracion vertical al comienzo del periodo de vel cte |
jjmedina | 0:a5367bd4e404 | 917 | float sgnAcelFin = 0.0; //Signo de la aceleracion vertical al final del periodo de vel cte |
jjmedina | 0:a5367bd4e404 | 918 | |
jjmedina | 0:a5367bd4e404 | 919 | const float tiempoMinimoArranque = 0.15; //Tiempo mínimo que puede durar la fase de aceleracion de la cabina |
jjmedina | 0:a5367bd4e404 | 920 | const float tiempoMinimoVelCte = 0.15; //Tiempo mínimo que puede durar el periodo de velocidad constante |
jjmedina | 0:a5367bd4e404 | 921 | const float tiempoMinimoFrenada = 0.15; //Tiempo mínimo que puede durar el periodo de deceleracion de la cabina |
jjmedina | 0:a5367bd4e404 | 922 | |
jjmedina | 0:a5367bd4e404 | 923 | 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;} |
jjmedina | 0:a5367bd4e404 | 924 | |
jjmedina | 0:a5367bd4e404 | 925 | FILE * fp; |
jjmedina | 0:a5367bd4e404 | 926 | |
jjmedina | 0:a5367bd4e404 | 927 | FILE * fp3; |
jjmedina | 0:a5367bd4e404 | 928 | |
jjmedina | 0:a5367bd4e404 | 929 | char NoViajel[2]; |
jjmedina | 0:a5367bd4e404 | 930 | char resultsFile[50]; |
jjmedina | 0:a5367bd4e404 | 931 | char dispVelFile[50]; |
jjmedina | 0:a5367bd4e404 | 932 | |
jjmedina | 0:a5367bd4e404 | 933 | int dif; |
jjmedina | 0:a5367bd4e404 | 934 | |
jjmedina | 0:a5367bd4e404 | 935 | //strcpy(resultsFile,"/local/IV_"); strcpy(dispVelFile,"/local/dva0_"); |
jjmedina | 0:a5367bd4e404 | 936 | |
jjmedina | 0:a5367bd4e404 | 937 | strcpy(resultsFile,"/local/raw_V_"); strcpy(dispVelFile,"/local/dva0_"); |
jjmedina | 0:a5367bd4e404 | 938 | |
jjmedina | 0:a5367bd4e404 | 939 | sprintf (NoViajel, "%d", ContadorEnsayos); |
jjmedina | 0:a5367bd4e404 | 940 | |
jjmedina | 0:a5367bd4e404 | 941 | strcat(resultsFile,NoViajel); strcat(dispVelFile,NoViajel); |
jjmedina | 0:a5367bd4e404 | 942 | strcat(resultsFile,".txt"); strcat(dispVelFile,".txt"); |
jjmedina | 0:a5367bd4e404 | 943 | |
jjmedina | 0:a5367bd4e404 | 944 | //fichero con todos los datos de la funcion anterior IV_ |
jjmedina | 0:a5367bd4e404 | 945 | 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); } |
jjmedina | 0:a5367bd4e404 | 946 | //fichero donde guardaron |
jjmedina | 0:a5367bd4e404 | 947 | 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); } |
jjmedina | 0:a5367bd4e404 | 948 | |
jjmedina | 0:a5367bd4e404 | 949 | //Lee, filtra y guarda las tres aceleraciones por separado |
jjmedina | 0:a5367bd4e404 | 950 | |
jjmedina | 0:a5367bd4e404 | 951 | for(unsigned int i = 0; i<ContadorDatos; i++) |
jjmedina | 0:a5367bd4e404 | 952 | { |
jjmedina | 0:a5367bd4e404 | 953 | 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()) |
jjmedina | 0:a5367bd4e404 | 954 | |
jjmedina | 0:a5367bd4e404 | 955 | /////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 956 | /// FILTRO DE BUTTERWORTH, PASO BAJO, ORDEN 2, 10Hz /// |
jjmedina | 0:a5367bd4e404 | 957 | /////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 958 | |
jjmedina | 0:a5367bd4e404 | 959 | if(i < 2) |
jjmedina | 0:a5367bd4e404 | 960 | { |
jjmedina | 0:a5367bd4e404 | 961 | xv[i+1] = aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 962 | yv[i+1] = aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 963 | } |
jjmedina | 0:a5367bd4e404 | 964 | else |
jjmedina | 0:a5367bd4e404 | 965 | { |
jjmedina | 0:a5367bd4e404 | 966 | xv[0] = xv[1]; xv[1] = xv[2]; |
jjmedina | 0:a5367bd4e404 | 967 | xv[2] = aXYZ[2] / gain; |
jjmedina | 0:a5367bd4e404 | 968 | |
jjmedina | 0:a5367bd4e404 | 969 | yv[0] = yv[1]; yv[1] = yv[2]; |
jjmedina | 0:a5367bd4e404 | 970 | yv[2] = (xv[0] + xv[2]) + 2.0 * xv[1] |
jjmedina | 0:a5367bd4e404 | 971 | + ( c_butt1 * yv[0]) + ( c_butt2 * yv[1]); |
jjmedina | 0:a5367bd4e404 | 972 | aXYZ[2] = yv[2]; |
jjmedina | 0:a5367bd4e404 | 973 | } |
jjmedina | 0:a5367bd4e404 | 974 | |
jjmedina | 0:a5367bd4e404 | 975 | ///////////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 976 | /// DETECCION INSTANTES DE ARRANQUE, COMIENZO Vcte, FIN Vcte Y PARADA /// |
jjmedina | 0:a5367bd4e404 | 977 | ///////////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 978 | |
jjmedina | 0:a5367bd4e404 | 979 | /// CALCULO DE ACELERACION Z DE REFERENCIA (DEBE COINCIDIR CON LA GRAVEDAD: 9.81) aZm0 |
jjmedina | 0:a5367bd4e404 | 980 | |
jjmedina | 0:a5367bd4e404 | 981 | if( (i >= ndes)&&(i < (ndes+nmed))) |
jjmedina | 0:a5367bd4e404 | 982 | { |
jjmedina | 0:a5367bd4e404 | 983 | aXm0 += aXYZ[0]/(float)nmed; |
jjmedina | 0:a5367bd4e404 | 984 | aYm0 += aXYZ[1]/(float)nmed; |
jjmedina | 0:a5367bd4e404 | 985 | aZm0 += aXYZ[2]/(float)nmed; |
jjmedina | 0:a5367bd4e404 | 986 | } |
jjmedina | 0:a5367bd4e404 | 987 | //Calculo nArr,nV0,nVfin,nPar |
jjmedina | 0:a5367bd4e404 | 988 | if( (i >= (ndes+nmed))) |
jjmedina | 0:a5367bd4e404 | 989 | { |
jjmedina | 0:a5367bd4e404 | 990 | if ((mov == false) && (fin == false)) |
jjmedina | 0:a5367bd4e404 | 991 | { |
jjmedina | 0:a5367bd4e404 | 992 | if(fabs(aXYZ[2]-aZm0) > fabs(tol*aZm0)) |
jjmedina | 0:a5367bd4e404 | 993 | { |
jjmedina | 0:a5367bd4e404 | 994 | nArr = i-(ndes+nmed); //Detecta el instante de arranque |
jjmedina | 0:a5367bd4e404 | 995 | mov = true; |
jjmedina | 0:a5367bd4e404 | 996 | t_Arr = t; |
jjmedina | 0:a5367bd4e404 | 997 | |
jjmedina | 0:a5367bd4e404 | 998 | sgnAcelArr = (aXYZ[2]-aZm0)/fabs(aXYZ[2]-aZm0); //Calcula el signo de la aceleracion vertical de arranque |
jjmedina | 0:a5367bd4e404 | 999 | |
jjmedina | 0:a5367bd4e404 | 1000 | //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); |
jjmedina | 0:a5367bd4e404 | 1001 | } |
jjmedina | 0:a5367bd4e404 | 1002 | } |
jjmedina | 0:a5367bd4e404 | 1003 | |
jjmedina | 0:a5367bd4e404 | 1004 | if ( (mov == true) && (velV == false) && (decel == false) && (t-t_Arr>tiempoMinimoArranque) ) |
jjmedina | 0:a5367bd4e404 | 1005 | { |
jjmedina | 0:a5367bd4e404 | 1006 | if(fabs(aXYZ[2]-aZm0) < fabs(tol*aZm0)) |
jjmedina | 0:a5367bd4e404 | 1007 | { |
jjmedina | 0:a5367bd4e404 | 1008 | sgnAcelv0 = (aXYZ[2]-aZm0)/fabs(aXYZ[2]-aZm0); |
jjmedina | 0:a5367bd4e404 | 1009 | if(fabs(sgnAcelv0-sgnAcelArr)>1.5) //El signo de la aceleracion de arranque tiene que ser distinto del signo de la de frenada |
jjmedina | 0:a5367bd4e404 | 1010 | { |
jjmedina | 0:a5367bd4e404 | 1011 | nV0 = i-(ndes+nmed); //Detecta el instante de inicio del periodo de V cte |
jjmedina | 0:a5367bd4e404 | 1012 | velV = true; |
jjmedina | 0:a5367bd4e404 | 1013 | t_V0 = t; |
jjmedina | 0:a5367bd4e404 | 1014 | //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); |
jjmedina | 0:a5367bd4e404 | 1015 | } |
jjmedina | 0:a5367bd4e404 | 1016 | } |
jjmedina | 0:a5367bd4e404 | 1017 | } |
jjmedina | 0:a5367bd4e404 | 1018 | |
jjmedina | 0:a5367bd4e404 | 1019 | if ( (mov == true) && (velV == true) && (decel == false) && (t-t_V0>tiempoMinimoVelCte)) |
jjmedina | 0:a5367bd4e404 | 1020 | { |
jjmedina | 0:a5367bd4e404 | 1021 | if(fabs(aXYZ[2]-aZm0) > fabs(tol*aZm0)) |
jjmedina | 0:a5367bd4e404 | 1022 | { |
jjmedina | 0:a5367bd4e404 | 1023 | sgnAcelFr = (aXYZ[2]-aZm0)/fabs(aXYZ[2]-aZm0); |
jjmedina | 0:a5367bd4e404 | 1024 | |
jjmedina | 0:a5367bd4e404 | 1025 | if(fabs(sgnAcelFr-sgnAcelArr)>1.5) //El signo de la aceleracion de arranque tiene que ser distinto del signo de la de frenada |
jjmedina | 0:a5367bd4e404 | 1026 | { |
jjmedina | 0:a5367bd4e404 | 1027 | nVfin = i-(ndes+nmed); //Detecta el instante de fin del periodo de V cte |
jjmedina | 0:a5367bd4e404 | 1028 | velV = false; |
jjmedina | 0:a5367bd4e404 | 1029 | decel = true; |
jjmedina | 0:a5367bd4e404 | 1030 | |
jjmedina | 0:a5367bd4e404 | 1031 | t_Vfin = t; |
jjmedina | 0:a5367bd4e404 | 1032 | |
jjmedina | 0:a5367bd4e404 | 1033 | //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); |
jjmedina | 0:a5367bd4e404 | 1034 | } |
jjmedina | 0:a5367bd4e404 | 1035 | } |
jjmedina | 0:a5367bd4e404 | 1036 | } |
jjmedina | 0:a5367bd4e404 | 1037 | |
jjmedina | 0:a5367bd4e404 | 1038 | if ( (mov == true) && (velV == false) && (decel == true) && (t-t_Vfin>tiempoMinimoFrenada)) |
jjmedina | 0:a5367bd4e404 | 1039 | { |
jjmedina | 0:a5367bd4e404 | 1040 | if(fabs(aXYZ[2]-aZm0) < fabs(tol*aZm0)) |
jjmedina | 0:a5367bd4e404 | 1041 | { |
jjmedina | 0:a5367bd4e404 | 1042 | sgnAcelFin = (aXYZ[2]-aZm0)/fabs(aXYZ[2]-aZm0); |
jjmedina | 0:a5367bd4e404 | 1043 | |
jjmedina | 0:a5367bd4e404 | 1044 | if(fabs(sgnAcelFin-sgnAcelFr)>1.5) |
jjmedina | 0:a5367bd4e404 | 1045 | { |
jjmedina | 0:a5367bd4e404 | 1046 | nPar = i-(ndes+nmed); //Detecta el instante de parada |
jjmedina | 0:a5367bd4e404 | 1047 | velV = false; |
jjmedina | 0:a5367bd4e404 | 1048 | decel = false; |
jjmedina | 0:a5367bd4e404 | 1049 | mov = false; |
jjmedina | 0:a5367bd4e404 | 1050 | fin = true; |
jjmedina | 0:a5367bd4e404 | 1051 | t_Par = t; |
jjmedina | 0:a5367bd4e404 | 1052 | //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); |
jjmedina | 0:a5367bd4e404 | 1053 | } |
jjmedina | 0:a5367bd4e404 | 1054 | } |
jjmedina | 0:a5367bd4e404 | 1055 | } |
jjmedina | 0:a5367bd4e404 | 1056 | } |
jjmedina | 0:a5367bd4e404 | 1057 | |
jjmedina | 0:a5367bd4e404 | 1058 | //////////////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 1059 | /// CALCULO DE POSICION Y VELOCIDAD INTEGRANDO LA ACELERACION Z FILTRADA /// |
jjmedina | 0:a5367bd4e404 | 1060 | //////////////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 1061 | |
jjmedina | 0:a5367bd4e404 | 1062 | int i2 = i-(ndes+nmed); //Nuevo idice para calcular posicion y velocidad |
jjmedina | 0:a5367bd4e404 | 1063 | |
jjmedina | 0:a5367bd4e404 | 1064 | |
jjmedina | 0:a5367bd4e404 | 1065 | |
jjmedina | 0:a5367bd4e404 | 1066 | //if (i2 == 0) {t_int = t;} //Ojo con los primeros valores |
jjmedina | 0:a5367bd4e404 | 1067 | |
jjmedina | 0:a5367bd4e404 | 1068 | //actualizo valores t_3, aZ_3 |
jjmedina | 0:a5367bd4e404 | 1069 | t_3[0] = t_3[1]; |
jjmedina | 0:a5367bd4e404 | 1070 | t_3[1] = t_3[2]; |
jjmedina | 0:a5367bd4e404 | 1071 | |
jjmedina | 0:a5367bd4e404 | 1072 | t_3[2] = t; |
jjmedina | 0:a5367bd4e404 | 1073 | |
jjmedina | 0:a5367bd4e404 | 1074 | aZ_3[0] = aZ_3[1]; |
jjmedina | 0:a5367bd4e404 | 1075 | aZ_3[1] = aZ_3[2]; |
jjmedina | 0:a5367bd4e404 | 1076 | aZ_3[2] = aXYZ[2]-aZm0; |
jjmedina | 0:a5367bd4e404 | 1077 | |
jjmedina | 0:a5367bd4e404 | 1078 | if(i2 >= 0) |
jjmedina | 0:a5367bd4e404 | 1079 | { |
jjmedina | 0:a5367bd4e404 | 1080 | vZ_3[0] = vZ_3[1]; |
jjmedina | 0:a5367bd4e404 | 1081 | vZ_3[1] = vZ_3[2]; |
jjmedina | 0:a5367bd4e404 | 1082 | |
jjmedina | 0:a5367bd4e404 | 1083 | xZ_3[0] = xZ_3[1]; |
jjmedina | 0:a5367bd4e404 | 1084 | xZ_3[1] = xZ_3[2]; |
jjmedina | 0:a5367bd4e404 | 1085 | |
jjmedina | 0:a5367bd4e404 | 1086 | xa = t_3[2]; |
jjmedina | 0:a5367bd4e404 | 1087 | xb = t_3[1]; |
jjmedina | 0:a5367bd4e404 | 1088 | xc = t_3[0]; |
jjmedina | 0:a5367bd4e404 | 1089 | |
jjmedina | 0:a5367bd4e404 | 1090 | h = (xa-xb)/2.0; |
jjmedina | 0:a5367bd4e404 | 1091 | |
jjmedina | 0:a5367bd4e404 | 1092 | fa = aZ_3[2]; |
jjmedina | 0:a5367bd4e404 | 1093 | fb = aZ_3[1]; |
jjmedina | 0:a5367bd4e404 | 1094 | fc = aZ_3[0]; |
jjmedina | 0:a5367bd4e404 | 1095 | |
jjmedina | 0:a5367bd4e404 | 1096 | //if(fabs(xa-xb)>1.0e-9) {fab = (fa-fb)/(xa-xb);} else {fab = 0.0;} |
jjmedina | 0:a5367bd4e404 | 1097 | //if(fabs(xb-xc)>1.0e-9) {fbc = (fb-fc)/(xb-xc);} else {fbc = 0.0;} |
jjmedina | 0:a5367bd4e404 | 1098 | //if(fabs(xa-xc)>1.0e-9) {fabc = (fab-fbc)/(xa-xc);} else {fabc = 0.0;} |
jjmedina | 0:a5367bd4e404 | 1099 | fab = (fa-fb)/(xa-xb); |
jjmedina | 0:a5367bd4e404 | 1100 | fbc = (fb-fc)/(xb-xc); |
jjmedina | 0:a5367bd4e404 | 1101 | fabc = (fab-fbc)/(xa-xc); |
jjmedina | 0:a5367bd4e404 | 1102 | |
jjmedina | 0:a5367bd4e404 | 1103 | vZ_3[2]= vZ_3[1] + fa*2.0*h + fab*(-2.0*h*h) + fabc*(-4.0*h*h*h/6.0); |
jjmedina | 0:a5367bd4e404 | 1104 | |
jjmedina | 0:a5367bd4e404 | 1105 | // Posicion |
jjmedina | 0:a5367bd4e404 | 1106 | |
jjmedina | 0:a5367bd4e404 | 1107 | fa = vZ_3[2]; |
jjmedina | 0:a5367bd4e404 | 1108 | fb = vZ_3[1]; |
jjmedina | 0:a5367bd4e404 | 1109 | fc = vZ_3[0]; |
jjmedina | 0:a5367bd4e404 | 1110 | |
jjmedina | 0:a5367bd4e404 | 1111 | //if(fabs(xa-xb)>1.0e-9) {fab = (fa-fb)/(xa-xb);} else {fab = 0.0;} |
jjmedina | 0:a5367bd4e404 | 1112 | //if(fabs(xb-xc)>1.0e-9) {fbc = (fb-fc)/(xb-xc);} else {fbc = 0.0;} |
jjmedina | 0:a5367bd4e404 | 1113 | //if(fabs(xa-xc)>1.0e-9) {fabc = (fab-fbc)/(xa-xc);} else {fabc = 0.0;} |
jjmedina | 0:a5367bd4e404 | 1114 | fab = (fa-fb)/(xa-xb); |
jjmedina | 0:a5367bd4e404 | 1115 | fbc = (fb-fc)/(xb-xc); |
jjmedina | 0:a5367bd4e404 | 1116 | fabc = (fab-fbc)/(xa-xc); |
jjmedina | 0:a5367bd4e404 | 1117 | |
jjmedina | 0:a5367bd4e404 | 1118 | xZ_3[2] = xZ_3[1] + fa*2.0*h + fab*(-2.0*h*h) + fabc*(-4.0*h*h*h/6.0); |
jjmedina | 0:a5367bd4e404 | 1119 | |
jjmedina | 0:a5367bd4e404 | 1120 | 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 |
jjmedina | 0:a5367bd4e404 | 1121 | } |
jjmedina | 0:a5367bd4e404 | 1122 | |
jjmedina | 0:a5367bd4e404 | 1123 | //CALCULO DE LOS DESPLAZAMIENTOS EN INSTANTES DE ARRANQUE Y PARADA |
jjmedina | 0:a5367bd4e404 | 1124 | |
jjmedina | 0:a5367bd4e404 | 1125 | if(i-(ndes+nmed) == nArr) {d_arr = xZ_3[2];} |
jjmedina | 0:a5367bd4e404 | 1126 | if(i-(ndes+nmed) == nPar) {d_par = xZ_3[2];} |
jjmedina | 0:a5367bd4e404 | 1127 | |
jjmedina | 0:a5367bd4e404 | 1128 | //CALCULO VELOCIDAD MEDIA EN INSTANTES FINALES |
jjmedina | 0:a5367bd4e404 | 1129 | //PARA IMPLEMENTAR EL TRUCO DE DANI |
jjmedina | 0:a5367bd4e404 | 1130 | |
jjmedina | 0:a5367bd4e404 | 1131 | if((ContadorDatos-i) <= nmedV) |
jjmedina | 0:a5367bd4e404 | 1132 | { |
jjmedina | 0:a5367bd4e404 | 1133 | vZmFin += vZ_3[2]/(float)nmedV; |
jjmedina | 0:a5367bd4e404 | 1134 | tFin = t_3[2]; |
jjmedina | 0:a5367bd4e404 | 1135 | } |
jjmedina | 0:a5367bd4e404 | 1136 | |
jjmedina | 0:a5367bd4e404 | 1137 | //Calculo de las velociades obtenidas por integración (que deben ser 0 en realidad) en los instantes de arranque y parada |
jjmedina | 0:a5367bd4e404 | 1138 | |
jjmedina | 0:a5367bd4e404 | 1139 | if(nArr == (i-(ndes+nmed))) {vArr = vZ_3[2];} |
jjmedina | 0:a5367bd4e404 | 1140 | if(nPar == (i-(ndes+nmed))) {vPar = vZ_3[2];} |
jjmedina | 0:a5367bd4e404 | 1141 | |
jjmedina | 0:a5367bd4e404 | 1142 | |
jjmedina | 0:a5367bd4e404 | 1143 | }//Fin loop principal |
jjmedina | 0:a5367bd4e404 | 1144 | |
jjmedina | 0:a5367bd4e404 | 1145 | |
jjmedina | 0:a5367bd4e404 | 1146 | |
jjmedina | 0:a5367bd4e404 | 1147 | fclose(fp); |
jjmedina | 0:a5367bd4e404 | 1148 | |
jjmedina | 0:a5367bd4e404 | 1149 | fclose(fp3); |
jjmedina | 0:a5367bd4e404 | 1150 | |
jjmedina | 0:a5367bd4e404 | 1151 | |
jjmedina | 0:a5367bd4e404 | 1152 | |
jjmedina | 0:a5367bd4e404 | 1153 | } |
jjmedina | 0:a5367bd4e404 | 1154 | |
jjmedina | 0:a5367bd4e404 | 1155 | void DatosConfort(unsigned int ContadorEnsayos, unsigned int ContadorDatos, int PasoTiempo) |
jjmedina | 0:a5367bd4e404 | 1156 | { |
jjmedina | 0:a5367bd4e404 | 1157 | float t; |
jjmedina | 0:a5367bd4e404 | 1158 | float dZ, vZ, aZ; |
jjmedina | 0:a5367bd4e404 | 1159 | |
jjmedina | 0:a5367bd4e404 | 1160 | const float tolV = 0.01; //Tolerancia, en tanto por uno de la velocidad nominal, para recalcular el tiempo final del viaje |
jjmedina | 0:a5367bd4e404 | 1161 | |
jjmedina | 0:a5367bd4e404 | 1162 | unsigned int RecalculaNPar = 0; //Se pone a 1 para recalcular el instante de parada |
jjmedina | 0:a5367bd4e404 | 1163 | unsigned int CalculaBo1 = 0; //Se pone a 1 para calcular el limite Bo1 |
jjmedina | 0:a5367bd4e404 | 1164 | unsigned int CalculaBo2 = 0; //Se pone a 1 para calcular el limite Bo2 |
jjmedina | 0:a5367bd4e404 | 1165 | |
jjmedina | 0:a5367bd4e404 | 1166 | Bo0 = 0; //Se establece el Bo0 al principio, tras los puntos descartados y los de calculo de g |
jjmedina | 0:a5367bd4e404 | 1167 | |
jjmedina | 0:a5367bd4e404 | 1168 | Bo3 = ContadorDatos-(ndes+nmed)-1; //Se establece el Bo3 al final, contando los puntos descartados y los de calculo de g |
jjmedina | 0:a5367bd4e404 | 1169 | if(Bo3>4000000000){Bo3 = 0;} |
jjmedina | 0:a5367bd4e404 | 1170 | |
jjmedina | 0:a5367bd4e404 | 1171 | Boacel = (unsigned int)(nV0 + nVfin)*0.5; //Se establece inicialmente (luego se vera si tenemos que cambiarlo) como la media entre nV0 y nV |
jjmedina | 0:a5367bd4e404 | 1172 | |
jjmedina | 0:a5367bd4e404 | 1173 | //float amax = 0.0;//VALOR POR PANTALLA |
jjmedina | 0:a5367bd4e404 | 1174 | //float dmax = 0.0;//VALOR POR PANTALLA |
jjmedina | 0:a5367bd4e404 | 1175 | //float Vmax = 0.0;//VALOR POR PANTALLA |
jjmedina | 0:a5367bd4e404 | 1176 | |
jjmedina | 0:a5367bd4e404 | 1177 | //Relativos al calculo del jerk |
jjmedina | 0:a5367bd4e404 | 1178 | const float running_time = 0.5; |
jjmedina | 0:a5367bd4e404 | 1179 | int Ai = (int)(0.5*running_time*1.0e6)/PasoTiempo; |
jjmedina | 0:a5367bd4e404 | 1180 | float aZ_05s[2*Ai+1]; //Aceleraciones dentro del periodo de 0.5 segundos para hacer regresion lineal |
jjmedina | 0:a5367bd4e404 | 1181 | float jerk_025s[Ai+1]; //Jerks dentro del periodo de 0.25 |
jjmedina | 0:a5367bd4e404 | 1182 | for (unsigned int i = 0; i<(2*Ai+1); i++) { aZ_05s[i] = 0.0; } |
jjmedina | 0:a5367bd4e404 | 1183 | for (unsigned int i = 0; i<(Ai+1); i++) { jerk_025s[i] = 0.0; } |
jjmedina | 0:a5367bd4e404 | 1184 | |
jjmedina | 0:a5367bd4e404 | 1185 | ////////////////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 1186 | // Datos para nueva integración de la velocidad para obtener desplazamiento // |
jjmedina | 0:a5367bd4e404 | 1187 | ////////////////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 1188 | |
jjmedina | 0:a5367bd4e404 | 1189 | float t_3[3], vZ_3[3], xZ_3[3]; //Datos guardados para hacer integracion numerica de aceleraciones |
jjmedina | 0:a5367bd4e404 | 1190 | float xa, xb, xc, h, fa, fb, fc, fab, fbc, fabc; |
jjmedina | 0:a5367bd4e404 | 1191 | |
jjmedina | 0:a5367bd4e404 | 1192 | |
jjmedina | 0:a5367bd4e404 | 1193 | for (unsigned int i = 0; i<3; i++) |
jjmedina | 0:a5367bd4e404 | 1194 | { |
jjmedina | 0:a5367bd4e404 | 1195 | t_3[i] = 0.0; vZ_3[i] = 0.0; xZ_3[i] = 0.0; |
jjmedina | 0:a5367bd4e404 | 1196 | } |
jjmedina | 0:a5367bd4e404 | 1197 | //nuevo Juan, soluciona error en nuevo calculo de dZ |
jjmedina | 0:a5367bd4e404 | 1198 | t_3[2] = (ndes + nmed - 1)*PasoTiempo/1000000.0; |
jjmedina | 0:a5367bd4e404 | 1199 | t_3[1] = (ndes + nmed - 2)*PasoTiempo/1000000.0; |
jjmedina | 0:a5367bd4e404 | 1200 | |
jjmedina | 0:a5367bd4e404 | 1201 | //Archivos de lectura y escritura |
jjmedina | 0:a5367bd4e404 | 1202 | |
jjmedina | 0:a5367bd4e404 | 1203 | FILE * fp; |
jjmedina | 0:a5367bd4e404 | 1204 | FILE * fp2; |
jjmedina | 0:a5367bd4e404 | 1205 | |
jjmedina | 0:a5367bd4e404 | 1206 | char NoViajel[2]; |
jjmedina | 0:a5367bd4e404 | 1207 | char dispVelFile[50]; char velMod[50]; |
jjmedina | 0:a5367bd4e404 | 1208 | |
jjmedina | 0:a5367bd4e404 | 1209 | strcpy(dispVelFile,"/local/dva0_"); strcpy(velMod,"/local/dvaj_"); |
jjmedina | 0:a5367bd4e404 | 1210 | |
jjmedina | 0:a5367bd4e404 | 1211 | sprintf (NoViajel, "%d", ContadorEnsayos); |
jjmedina | 0:a5367bd4e404 | 1212 | strcat(dispVelFile,NoViajel); strcat(velMod,NoViajel); |
jjmedina | 0:a5367bd4e404 | 1213 | strcat(dispVelFile,".txt"); strcat(velMod,".txt"); |
jjmedina | 0:a5367bd4e404 | 1214 | |
jjmedina | 0:a5367bd4e404 | 1215 | fp = fopen(dispVelFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", dispVelFile); } |
jjmedina | 0:a5367bd4e404 | 1216 | fp2 = fopen(velMod,"w"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", velMod); } |
jjmedina | 0:a5367bd4e404 | 1217 | |
jjmedina | 0:a5367bd4e404 | 1218 | |
jjmedina | 0:a5367bd4e404 | 1219 | //Lee velocidad para establecer periodos y recalcular nPar. Tambnien se calculan Bo1 y Bo2 |
jjmedina | 0:a5367bd4e404 | 1220 | |
jjmedina | 0:a5367bd4e404 | 1221 | int nDatos = (ContadorDatos-(ndes+nmed)); |
jjmedina | 0:a5367bd4e404 | 1222 | if (nDatos < 0) {nDatos = 0;} |
jjmedina | 0:a5367bd4e404 | 1223 | |
jjmedina | 0:a5367bd4e404 | 1224 | float t_in; |
jjmedina | 0:a5367bd4e404 | 1225 | 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 |
jjmedina | 0:a5367bd4e404 | 1226 | 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 |
jjmedina | 0:a5367bd4e404 | 1227 | |
jjmedina | 0:a5367bd4e404 | 1228 | for(int i = 0; i < nDatos; i++) |
jjmedina | 0:a5367bd4e404 | 1229 | { |
jjmedina | 0:a5367bd4e404 | 1230 | fscanf(fp,"%f %f %f %f", &t, &dZ, &vZ, &aZ);//aZ filtered, aX y aY raw. |
jjmedina | 0:a5367bd4e404 | 1231 | |
jjmedina | 0:a5367bd4e404 | 1232 | if(i == 0) { t_in = t;} |
jjmedina | 0:a5367bd4e404 | 1233 | |
jjmedina | 0:a5367bd4e404 | 1234 | //vZ = vZ - vZmFin*(t/tFin); //CAMBIO JL (11/10/2013) CON ESTA FORMA LA VELOCIDAD NO PARTE DE 0 |
jjmedina | 0:a5367bd4e404 | 1235 | //vZ = vZ - vZmFin*((t-t_in)/(tFin-t_in)); |
jjmedina | 0:a5367bd4e404 | 1236 | |
jjmedina | 0:a5367bd4e404 | 1237 | //Truco de Dani mas sofisticado |
jjmedina | 0:a5367bd4e404 | 1238 | if(i < nArr) { vZ = vZ - vArr*((t-t_in)/(t_Arr-t_in));} |
jjmedina | 0:a5367bd4e404 | 1239 | if(i >= nArr) { vZ = vZ - (a_LV*t + b_LV);} |
jjmedina | 0:a5367bd4e404 | 1240 | |
jjmedina | 0:a5367bd4e404 | 1241 | |
jjmedina | 0:a5367bd4e404 | 1242 | // NUEVA INTEGRACIÓN DE LA VELOCIDAD PARA CALCULAR DESPLAZAMIENTO |
jjmedina | 0:a5367bd4e404 | 1243 | |
jjmedina | 0:a5367bd4e404 | 1244 | t_3[0] = t_3[1]; |
jjmedina | 0:a5367bd4e404 | 1245 | t_3[1] = t_3[2]; |
jjmedina | 0:a5367bd4e404 | 1246 | t_3[2] = t; |
jjmedina | 0:a5367bd4e404 | 1247 | |
jjmedina | 0:a5367bd4e404 | 1248 | vZ_3[0] = vZ_3[1]; |
jjmedina | 0:a5367bd4e404 | 1249 | vZ_3[1] = vZ_3[2]; |
jjmedina | 0:a5367bd4e404 | 1250 | vZ_3[2] = vZ; |
jjmedina | 0:a5367bd4e404 | 1251 | |
jjmedina | 0:a5367bd4e404 | 1252 | xZ_3[0] = xZ_3[1]; |
jjmedina | 0:a5367bd4e404 | 1253 | xZ_3[1] = xZ_3[2]; |
jjmedina | 0:a5367bd4e404 | 1254 | |
jjmedina | 0:a5367bd4e404 | 1255 | xa = t_3[2]; |
jjmedina | 0:a5367bd4e404 | 1256 | xb = t_3[1]; |
jjmedina | 0:a5367bd4e404 | 1257 | xc = t_3[0]; |
jjmedina | 0:a5367bd4e404 | 1258 | |
jjmedina | 0:a5367bd4e404 | 1259 | h = (xa-xb)/2.0; |
jjmedina | 0:a5367bd4e404 | 1260 | |
jjmedina | 0:a5367bd4e404 | 1261 | fa = vZ_3[2]; |
jjmedina | 0:a5367bd4e404 | 1262 | fb = vZ_3[1]; |
jjmedina | 0:a5367bd4e404 | 1263 | fc = vZ_3[0]; |
jjmedina | 0:a5367bd4e404 | 1264 | |
jjmedina | 0:a5367bd4e404 | 1265 | //if(fabs(xa-xb)>1.0e-9) {fab = (fa-fb)/(xa-xb);} else {fab = 0.0;} |
jjmedina | 0:a5367bd4e404 | 1266 | //if(fabs(xb-xc)>1.0e-9) {fbc = (fb-fc)/(xb-xc);} else {fbc = 0.0;} |
jjmedina | 0:a5367bd4e404 | 1267 | //if(fabs(xa-xc)>1.0e-9) {fabc = (fab-fbc)/(xa-xc);} else {fabc = 0.0;} |
jjmedina | 0:a5367bd4e404 | 1268 | fab = (fa-fb)/(xa-xb); |
jjmedina | 0:a5367bd4e404 | 1269 | fbc = (fb-fc)/(xb-xc); |
jjmedina | 0:a5367bd4e404 | 1270 | fabc = (fab-fbc)/(xa-xc); |
jjmedina | 0:a5367bd4e404 | 1271 | |
jjmedina | 0:a5367bd4e404 | 1272 | xZ_3[2] = xZ_3[1] + fa*2.0*h + fab*(-2.0*h*h) + fabc*(-4.0*h*h*h/6.0); |
jjmedina | 0:a5367bd4e404 | 1273 | |
jjmedina | 0:a5367bd4e404 | 1274 | dZ = xZ_3[2]; |
jjmedina | 0:a5367bd4e404 | 1275 | |
jjmedina | 0:a5367bd4e404 | 1276 | if(i == nArr) {d_arr = dZ;} |
jjmedina | 0:a5367bd4e404 | 1277 | if(i == nPar) {d_par = dZ;} |
jjmedina | 0:a5367bd4e404 | 1278 | |
jjmedina | 0:a5367bd4e404 | 1279 | // HASTA AQUÍ |
jjmedina | 0:a5367bd4e404 | 1280 | |
jjmedina | 0:a5367bd4e404 | 1281 | |
jjmedina | 0:a5367bd4e404 | 1282 | if( (i>=nV0)&&(i<nVfin) ) |
jjmedina | 0:a5367bd4e404 | 1283 | { |
jjmedina | 0:a5367bd4e404 | 1284 | vNom += fabs(vZ)/(float)(nVfin-nV0); |
jjmedina | 0:a5367bd4e404 | 1285 | } |
jjmedina | 0:a5367bd4e404 | 1286 | |
jjmedina | 0:a5367bd4e404 | 1287 | if( (i == nPar) && (fabs(vZ) > tolV*fabs(vNom)) ) |
jjmedina | 0:a5367bd4e404 | 1288 | { |
jjmedina | 0:a5367bd4e404 | 1289 | RecalculaNPar = 1; |
jjmedina | 0:a5367bd4e404 | 1290 | } |
jjmedina | 0:a5367bd4e404 | 1291 | |
jjmedina | 0:a5367bd4e404 | 1292 | if( (RecalculaNPar == 1) && (fabs(vZ) < tolV*fabs(vNom)) ) |
jjmedina | 0:a5367bd4e404 | 1293 | { |
jjmedina | 0:a5367bd4e404 | 1294 | nPar = i; |
jjmedina | 0:a5367bd4e404 | 1295 | t_Par = t; |
jjmedina | 0:a5367bd4e404 | 1296 | RecalculaNPar = 0; |
jjmedina | 0:a5367bd4e404 | 1297 | } |
jjmedina | 0:a5367bd4e404 | 1298 | |
jjmedina | 0:a5367bd4e404 | 1299 | // Calculo de Bo1 |
jjmedina | 0:a5367bd4e404 | 1300 | |
jjmedina | 0:a5367bd4e404 | 1301 | if( i == nArr) { CalculaBo1 = 1; } |
jjmedina | 0:a5367bd4e404 | 1302 | |
jjmedina | 0:a5367bd4e404 | 1303 | if ((fabs(fabs(dZ)-fabs(d_arr))>0.5) && (CalculaBo1 == 1)) |
jjmedina | 0:a5367bd4e404 | 1304 | { |
jjmedina | 0:a5367bd4e404 | 1305 | Bo1 = i; |
jjmedina | 0:a5367bd4e404 | 1306 | CalculaBo1 = 0; |
jjmedina | 0:a5367bd4e404 | 1307 | } |
jjmedina | 0:a5367bd4e404 | 1308 | //OJO: INTENTAR CORREGIR Bo1 CON NUEVO PUNTO DE PARADA arranque |
jjmedina | 0:a5367bd4e404 | 1309 | |
jjmedina | 0:a5367bd4e404 | 1310 | // Calculo de Bo2 |
jjmedina | 0:a5367bd4e404 | 1311 | |
jjmedina | 0:a5367bd4e404 | 1312 | //OJO: INTENTAR CORREGIR Bo2 CON NUEVO PUNTO DE PARADA nPar |
jjmedina | 0:a5367bd4e404 | 1313 | |
jjmedina | 0:a5367bd4e404 | 1314 | if( i == nV0) { CalculaBo2 = 1; } |
jjmedina | 0:a5367bd4e404 | 1315 | |
jjmedina | 0:a5367bd4e404 | 1316 | if ((fabs(fabs(dZ)-fabs(d_par))<0.5) && (CalculaBo2 == 1) && (i>Bo1)) |
jjmedina | 0:a5367bd4e404 | 1317 | { |
jjmedina | 0:a5367bd4e404 | 1318 | Bo2 = i; |
jjmedina | 0:a5367bd4e404 | 1319 | CalculaBo2 = 0; |
jjmedina | 0:a5367bd4e404 | 1320 | } |
jjmedina | 0:a5367bd4e404 | 1321 | |
jjmedina | 0:a5367bd4e404 | 1322 | //Calculo maxima aceleracion y deceleracion. Calcula Vmax1 y Vmax2. n05Vmaxi y n95Vmaxi en la siguiente pasada. |
jjmedina | 0:a5367bd4e404 | 1323 | |
jjmedina | 0:a5367bd4e404 | 1324 | if ( (i>=Bo0) && (i<Boacel)) |
jjmedina | 0:a5367bd4e404 | 1325 | { |
jjmedina | 0:a5367bd4e404 | 1326 | if(fabs(aZ)>amax) { amax = fabs(aZ);} |
jjmedina | 0:a5367bd4e404 | 1327 | if(fabs(vZ)>Vmax1) { Vmax1 = fabs(vZ);} |
jjmedina | 0:a5367bd4e404 | 1328 | } |
jjmedina | 0:a5367bd4e404 | 1329 | |
jjmedina | 0:a5367bd4e404 | 1330 | if ( (i>=Boacel) && (i<=Bo3)) |
jjmedina | 0:a5367bd4e404 | 1331 | { |
jjmedina | 0:a5367bd4e404 | 1332 | if(fabs(aZ)>dmax) { dmax = fabs(aZ);} |
jjmedina | 0:a5367bd4e404 | 1333 | if(fabs(vZ)>Vmax2) { Vmax2 = fabs(vZ);} |
jjmedina | 0:a5367bd4e404 | 1334 | } |
jjmedina | 0:a5367bd4e404 | 1335 | |
jjmedina | 0:a5367bd4e404 | 1336 | //Calculo de jerk por regresion |
jjmedina | 0:a5367bd4e404 | 1337 | |
jjmedina | 0:a5367bd4e404 | 1338 | for(unsigned int j = 0; j<(2*Ai+1); j++) { aZ_05s[j] = aZ_05s[j+1]; } |
jjmedina | 0:a5367bd4e404 | 1339 | aZ_05s[2*Ai] = aZ; |
jjmedina | 0:a5367bd4e404 | 1340 | for(unsigned int j = 0; j<(Ai+1); j++) { jerk_025s[j] = jerk_025s[j+1]; } |
jjmedina | 0:a5367bd4e404 | 1341 | jerk_025s[Ai] = RegresionJerk(aZ_05s, 2*Ai+1); |
jjmedina | 0:a5367bd4e404 | 1342 | |
jjmedina | 0:a5367bd4e404 | 1343 | //Ojo, el jerk esta desfasado 0.25 segundos. Claculo su m�xdimo en la siguiente pasada. |
jjmedina | 0:a5367bd4e404 | 1344 | |
jjmedina | 0:a5367bd4e404 | 1345 | //calculo Vmax a partir de Vmax1 y Vmax2. |
jjmedina | 0:a5367bd4e404 | 1346 | if (Vmax1<Vmax2) |
jjmedina | 0:a5367bd4e404 | 1347 | { |
jjmedina | 0:a5367bd4e404 | 1348 | Vmax = Vmax2; |
jjmedina | 0:a5367bd4e404 | 1349 | }else |
jjmedina | 0:a5367bd4e404 | 1350 | { |
jjmedina | 0:a5367bd4e404 | 1351 | Vmax = Vmax1; |
jjmedina | 0:a5367bd4e404 | 1352 | } |
jjmedina | 0:a5367bd4e404 | 1353 | |
jjmedina | 0:a5367bd4e404 | 1354 | if(i== nPar) {d_par = dZ;} |
jjmedina | 0:a5367bd4e404 | 1355 | |
jjmedina | 0:a5367bd4e404 | 1356 | |
jjmedina | 0:a5367bd4e404 | 1357 | fprintf(fp2,"%f %f %f %f %f \n", t, dZ, vZ, aZ, jerk_025s[0]); |
jjmedina | 0:a5367bd4e404 | 1358 | } |
jjmedina | 0:a5367bd4e404 | 1359 | |
jjmedina | 0:a5367bd4e404 | 1360 | |
jjmedina | 0:a5367bd4e404 | 1361 | |
jjmedina | 0:a5367bd4e404 | 1362 | |
jjmedina | 0:a5367bd4e404 | 1363 | fclose(fp); |
jjmedina | 0:a5367bd4e404 | 1364 | fclose(fp2); |
jjmedina | 0:a5367bd4e404 | 1365 | |
jjmedina | 0:a5367bd4e404 | 1366 | |
jjmedina | 0:a5367bd4e404 | 1367 | |
jjmedina | 0:a5367bd4e404 | 1368 | //ELIMINA ARCHIVO dva0 |
jjmedina | 0:a5367bd4e404 | 1369 | int rmDVA0 = remove(dispVelFile); if(rmDVA0 != 0) { display.Clear();display.SetXY(0,0); display.printf("Error removing %s file\n", dispVelFile); } |
jjmedina | 0:a5367bd4e404 | 1370 | |
jjmedina | 0:a5367bd4e404 | 1371 | } |
jjmedina | 0:a5367bd4e404 | 1372 | |
jjmedina | 0:a5367bd4e404 | 1373 | void calcula_n05n95_Boveloc_jerkmax(unsigned int ContadorEnsayos, unsigned int ContadorDatos, int PasoTiempo) |
jjmedina | 0:a5367bd4e404 | 1374 | { |
jjmedina | 0:a5367bd4e404 | 1375 | |
jjmedina | 0:a5367bd4e404 | 1376 | //float jerkmax = 0.0;//POR PANTALLA |
jjmedina | 0:a5367bd4e404 | 1377 | float t; |
jjmedina | 0:a5367bd4e404 | 1378 | float dZ, vZ, aZ, jerk; |
jjmedina | 0:a5367bd4e404 | 1379 | int CalculaBo2(0); |
jjmedina | 0:a5367bd4e404 | 1380 | |
jjmedina | 0:a5367bd4e404 | 1381 | //5% and 95% of Vmax1 y Vmax2 |
jjmedina | 0:a5367bd4e404 | 1382 | unsigned int c1(0);unsigned int c2(0);unsigned int c3(0);unsigned int c4(0); |
jjmedina | 0:a5367bd4e404 | 1383 | |
jjmedina | 0:a5367bd4e404 | 1384 | |
jjmedina | 0:a5367bd4e404 | 1385 | //Velocidad A95 |
jjmedina | 0:a5367bd4e404 | 1386 | const float A95max_veloc_acel = 0.95*Vmax1; |
jjmedina | 0:a5367bd4e404 | 1387 | const float A95max_veloc_decel = 0.95*Vmax2; |
jjmedina | 0:a5367bd4e404 | 1388 | unsigned int contA95 = Bo1; |
jjmedina | 0:a5367bd4e404 | 1389 | unsigned int contA95_2 = Boacel + 1; |
jjmedina | 0:a5367bd4e404 | 1390 | bool Bo_veloc_acel_done = false; |
jjmedina | 0:a5367bd4e404 | 1391 | bool Bo_veloc_decel_done = false; |
jjmedina | 0:a5367bd4e404 | 1392 | float t2(0.0); |
jjmedina | 0:a5367bd4e404 | 1393 | float t3(0.0); |
jjmedina | 0:a5367bd4e404 | 1394 | |
jjmedina | 0:a5367bd4e404 | 1395 | //Archivos de lectura y escritura |
jjmedina | 0:a5367bd4e404 | 1396 | |
jjmedina | 0:a5367bd4e404 | 1397 | FILE * fp; |
jjmedina | 0:a5367bd4e404 | 1398 | |
jjmedina | 0:a5367bd4e404 | 1399 | char NoViajel[2]; |
jjmedina | 0:a5367bd4e404 | 1400 | char velMod[50]; |
jjmedina | 0:a5367bd4e404 | 1401 | |
jjmedina | 0:a5367bd4e404 | 1402 | strcpy(velMod,"/local/dvaj_"); |
jjmedina | 0:a5367bd4e404 | 1403 | |
jjmedina | 0:a5367bd4e404 | 1404 | sprintf (NoViajel, "%d", ContadorEnsayos); |
jjmedina | 0:a5367bd4e404 | 1405 | strcat(velMod,NoViajel); |
jjmedina | 0:a5367bd4e404 | 1406 | strcat(velMod,".txt"); |
jjmedina | 0:a5367bd4e404 | 1407 | |
jjmedina | 0:a5367bd4e404 | 1408 | fp = fopen(velMod,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", velMod); } |
jjmedina | 0:a5367bd4e404 | 1409 | |
jjmedina | 0:a5367bd4e404 | 1410 | int nDatos = (ContadorDatos-(ndes+nmed)); |
jjmedina | 0:a5367bd4e404 | 1411 | if (nDatos < 0) {nDatos = 0;} |
jjmedina | 0:a5367bd4e404 | 1412 | |
jjmedina | 0:a5367bd4e404 | 1413 | //loop principal |
jjmedina | 0:a5367bd4e404 | 1414 | for(unsigned int i = 0; i<nDatos; i++) |
jjmedina | 0:a5367bd4e404 | 1415 | { |
jjmedina | 0:a5367bd4e404 | 1416 | //read data row |
jjmedina | 0:a5367bd4e404 | 1417 | fscanf(fp,"%f %f %f %f %f", &t, &dZ, &vZ, &aZ, &jerk); |
jjmedina | 0:a5367bd4e404 | 1418 | |
jjmedina | 0:a5367bd4e404 | 1419 | if( i == nV0) { CalculaBo2 = 1; } |
jjmedina | 0:a5367bd4e404 | 1420 | |
jjmedina | 0:a5367bd4e404 | 1421 | if ( (fabs(fabs(dZ)-fabs(d_par))<0.5) && (i>Bo1) && (CalculaBo2 == 1) ) |
jjmedina | 0:a5367bd4e404 | 1422 | { |
jjmedina | 0:a5367bd4e404 | 1423 | Bo2 = i; |
jjmedina | 0:a5367bd4e404 | 1424 | CalculaBo2 = 0; |
jjmedina | 0:a5367bd4e404 | 1425 | } |
jjmedina | 0:a5367bd4e404 | 1426 | |
jjmedina | 0:a5367bd4e404 | 1427 | //jerkmax |
jjmedina | 0:a5367bd4e404 | 1428 | if ( fabs(jerk) >= jerkmax ) |
jjmedina | 0:a5367bd4e404 | 1429 | { |
jjmedina | 0:a5367bd4e404 | 1430 | jerkmax = fabs(jerk); |
jjmedina | 0:a5367bd4e404 | 1431 | } |
jjmedina | 0:a5367bd4e404 | 1432 | |
jjmedina | 0:a5367bd4e404 | 1433 | //5% and 95% of Vmax1 |
jjmedina | 0:a5367bd4e404 | 1434 | if ( (i >= Bo0) && (i <= Boacel) ) |
jjmedina | 0:a5367bd4e404 | 1435 | { |
jjmedina | 0:a5367bd4e404 | 1436 | if( ( fabs(vZ) > 0.05*Vmax1 ) && (c1 == 0) ) |
jjmedina | 0:a5367bd4e404 | 1437 | { |
jjmedina | 0:a5367bd4e404 | 1438 | n05Vmax1 = i; |
jjmedina | 0:a5367bd4e404 | 1439 | c1 = 1; |
jjmedina | 0:a5367bd4e404 | 1440 | } |
jjmedina | 0:a5367bd4e404 | 1441 | if( ( fabs(vZ) > 0.95*Vmax1 ) && (c2 == 0) ) |
jjmedina | 0:a5367bd4e404 | 1442 | { |
jjmedina | 0:a5367bd4e404 | 1443 | n95Vmax1 = i; |
jjmedina | 0:a5367bd4e404 | 1444 | c2 = 1; |
jjmedina | 0:a5367bd4e404 | 1445 | } |
jjmedina | 0:a5367bd4e404 | 1446 | } |
jjmedina | 0:a5367bd4e404 | 1447 | |
jjmedina | 0:a5367bd4e404 | 1448 | //5% and 95% of Vmax2 |
jjmedina | 0:a5367bd4e404 | 1449 | if ( (i >= (Boacel + 1)) && (i < Bo3) ) |
jjmedina | 0:a5367bd4e404 | 1450 | { |
jjmedina | 0:a5367bd4e404 | 1451 | if( ( fabs(vZ) < 0.95*Vmax2 ) && (c3 == 0) ) |
jjmedina | 0:a5367bd4e404 | 1452 | { |
jjmedina | 0:a5367bd4e404 | 1453 | n95Vmax2 = i; |
jjmedina | 0:a5367bd4e404 | 1454 | c3 = 1; |
jjmedina | 0:a5367bd4e404 | 1455 | } |
jjmedina | 0:a5367bd4e404 | 1456 | if( ( fabs(vZ) < 0.05*Vmax2 ) && (c4 == 0) ) |
jjmedina | 0:a5367bd4e404 | 1457 | { |
jjmedina | 0:a5367bd4e404 | 1458 | n05Vmax2 = i; |
jjmedina | 0:a5367bd4e404 | 1459 | c4 = 1; |
jjmedina | 0:a5367bd4e404 | 1460 | } |
jjmedina | 0:a5367bd4e404 | 1461 | } |
jjmedina | 0:a5367bd4e404 | 1462 | |
jjmedina | 0:a5367bd4e404 | 1463 | //Bo_veloc_acel y Bo_veloc_decel |
jjmedina | 0:a5367bd4e404 | 1464 | if( ( (i > Bo1) && (i<=Boacel) ) && ( Bo_veloc_acel_done == false ) ) |
jjmedina | 0:a5367bd4e404 | 1465 | { |
jjmedina | 0:a5367bd4e404 | 1466 | if ( fabs(vZ) >= A95max_veloc_acel ) |
jjmedina | 0:a5367bd4e404 | 1467 | { |
jjmedina | 0:a5367bd4e404 | 1468 | Bo_veloc_acel = contA95; |
jjmedina | 0:a5367bd4e404 | 1469 | t2 = t + 1.0; |
jjmedina | 0:a5367bd4e404 | 1470 | Bo_veloc_acel_done = true; |
jjmedina | 0:a5367bd4e404 | 1471 | } |
jjmedina | 0:a5367bd4e404 | 1472 | contA95++; |
jjmedina | 0:a5367bd4e404 | 1473 | } |
jjmedina | 0:a5367bd4e404 | 1474 | if( ( (i > (Boacel + 1)) && (i<=Bo3)) && (Bo_veloc_decel_done == false) ) |
jjmedina | 0:a5367bd4e404 | 1475 | { |
jjmedina | 0:a5367bd4e404 | 1476 | if ( fabs(vZ) >= A95max_veloc_decel ) |
jjmedina | 0:a5367bd4e404 | 1477 | { |
jjmedina | 0:a5367bd4e404 | 1478 | Bo_veloc_decel = contA95_2; |
jjmedina | 0:a5367bd4e404 | 1479 | t3 = t - 1.0; |
jjmedina | 0:a5367bd4e404 | 1480 | Bo_veloc_decel_done = true; |
jjmedina | 0:a5367bd4e404 | 1481 | } |
jjmedina | 0:a5367bd4e404 | 1482 | contA95_2++; |
jjmedina | 0:a5367bd4e404 | 1483 | } |
jjmedina | 0:a5367bd4e404 | 1484 | |
jjmedina | 0:a5367bd4e404 | 1485 | |
jjmedina | 0:a5367bd4e404 | 1486 | }//loop principal |
jjmedina | 0:a5367bd4e404 | 1487 | |
jjmedina | 0:a5367bd4e404 | 1488 | //calculos after loop |
jjmedina | 0:a5367bd4e404 | 1489 | //t2 y t3 |
jjmedina | 0:a5367bd4e404 | 1490 | |
jjmedina | 0:a5367bd4e404 | 1491 | for (unsigned int j = 0 ; j<=100 ; j++) |
jjmedina | 0:a5367bd4e404 | 1492 | { |
jjmedina | 0:a5367bd4e404 | 1493 | if(t3<t2) |
jjmedina | 0:a5367bd4e404 | 1494 | { |
jjmedina | 0:a5367bd4e404 | 1495 | t2 = t2 - ((float)(1))/10.0; |
jjmedina | 0:a5367bd4e404 | 1496 | t3 = t3 + ((float)(1))/10.0; |
jjmedina | 0:a5367bd4e404 | 1497 | }else |
jjmedina | 0:a5367bd4e404 | 1498 | { |
jjmedina | 0:a5367bd4e404 | 1499 | Bo_veloc_acel = (unsigned int)( Bo_veloc_acel + (1.0/((float)(PasoTiempo/1000000.0))) - ((float)(j))/(10.0*((float)(PasoTiempo/1000000.0))) ); |
jjmedina | 0:a5367bd4e404 | 1500 | Bo_veloc_decel = (unsigned int)( Bo_veloc_decel - (1.0/((float)(PasoTiempo/1000000.0))) + ((float)(j))/(10.0*((float)(PasoTiempo/1000000.0))) ); |
jjmedina | 0:a5367bd4e404 | 1501 | break; |
jjmedina | 0:a5367bd4e404 | 1502 | } |
jjmedina | 0:a5367bd4e404 | 1503 | } |
jjmedina | 0:a5367bd4e404 | 1504 | |
jjmedina | 0:a5367bd4e404 | 1505 | |
jjmedina | 0:a5367bd4e404 | 1506 | |
jjmedina | 0:a5367bd4e404 | 1507 | |
jjmedina | 0:a5367bd4e404 | 1508 | //seguridad |
jjmedina | 0:a5367bd4e404 | 1509 | //seguridad por si nVmax1 |
jjmedina | 0:a5367bd4e404 | 1510 | if ( c1 == 0 ) |
jjmedina | 0:a5367bd4e404 | 1511 | { |
jjmedina | 0:a5367bd4e404 | 1512 | n05Vmax1 = (unsigned int)(Bo0 + (Bo1-Bo0)/2.0); |
jjmedina | 0:a5367bd4e404 | 1513 | } |
jjmedina | 0:a5367bd4e404 | 1514 | if ( c2 == 0 ) |
jjmedina | 0:a5367bd4e404 | 1515 | { |
jjmedina | 0:a5367bd4e404 | 1516 | n95Vmax1 = Boacel; |
jjmedina | 0:a5367bd4e404 | 1517 | } |
jjmedina | 0:a5367bd4e404 | 1518 | //seguridad por si nVmax2 |
jjmedina | 0:a5367bd4e404 | 1519 | if ( c3 == 0 ) |
jjmedina | 0:a5367bd4e404 | 1520 | { |
jjmedina | 0:a5367bd4e404 | 1521 | n95Vmax2 = Boacel; |
jjmedina | 0:a5367bd4e404 | 1522 | } |
jjmedina | 0:a5367bd4e404 | 1523 | if ( c4 == 0 ) |
jjmedina | 0:a5367bd4e404 | 1524 | { |
jjmedina | 0:a5367bd4e404 | 1525 | n05Vmax2 = (unsigned int)(Bo2 + (Bo3-Bo2)/2.0); |
jjmedina | 0:a5367bd4e404 | 1526 | } |
jjmedina | 0:a5367bd4e404 | 1527 | //cierra ficheros |
jjmedina | 0:a5367bd4e404 | 1528 | fclose(fp); |
jjmedina | 0:a5367bd4e404 | 1529 | |
jjmedina | 0:a5367bd4e404 | 1530 | |
jjmedina | 0:a5367bd4e404 | 1531 | } |
jjmedina | 0:a5367bd4e404 | 1532 | |
jjmedina | 0:a5367bd4e404 | 1533 | |
jjmedina | 0:a5367bd4e404 | 1534 | |
jjmedina | 0:a5367bd4e404 | 1535 | void calcula_A95_V_acel_decel(unsigned int ContadorEnsayos, unsigned int ContadorDatos, int PasoTiempo) |
jjmedina | 0:a5367bd4e404 | 1536 | |
jjmedina | 0:a5367bd4e404 | 1537 | { |
jjmedina | 0:a5367bd4e404 | 1538 | float t; |
jjmedina | 0:a5367bd4e404 | 1539 | float dZ, vZ, aZ, jerk; |
jjmedina | 0:a5367bd4e404 | 1540 | |
jjmedina | 0:a5367bd4e404 | 1541 | //VA95 |
jjmedina | 0:a5367bd4e404 | 1542 | int length_maximoVA95(0); |
jjmedina | 0:a5367bd4e404 | 1543 | if( Bo_veloc_decel >= Bo_veloc_acel ) |
jjmedina | 0:a5367bd4e404 | 1544 | { |
jjmedina | 0:a5367bd4e404 | 1545 | length_maximoVA95 = (int)((Bo_veloc_decel - Bo_veloc_acel)*0.05) + 1; |
jjmedina | 0:a5367bd4e404 | 1546 | }else |
jjmedina | 0:a5367bd4e404 | 1547 | { |
jjmedina | 0:a5367bd4e404 | 1548 | length_maximoVA95 = 1; |
jjmedina | 0:a5367bd4e404 | 1549 | } |
jjmedina | 0:a5367bd4e404 | 1550 | //int length_maximoVA95 = (int)((Bo_veloc_decel - Bo_veloc_acel)*0.05) + 1; |
jjmedina | 0:a5367bd4e404 | 1551 | |
jjmedina | 0:a5367bd4e404 | 1552 | int maxindex(0); |
jjmedina | 0:a5367bd4e404 | 1553 | vector<float> *maximoAV95 = new vector<float> (length_maximoVA95,0.0); |
jjmedina | 0:a5367bd4e404 | 1554 | //vector<float> maximoAV95 (length_maximoVA95,0.0); |
jjmedina | 0:a5367bd4e404 | 1555 | vector<float> *diferencia_vec = new vector<float>; |
jjmedina | 0:a5367bd4e404 | 1556 | vector<float>::iterator up; |
jjmedina | 0:a5367bd4e404 | 1557 | float diferencia; |
jjmedina | 0:a5367bd4e404 | 1558 | float maximo; |
jjmedina | 0:a5367bd4e404 | 1559 | int contadorA95(0); |
jjmedina | 0:a5367bd4e404 | 1560 | //aA95 |
jjmedina | 0:a5367bd4e404 | 1561 | int length_maximo_aA95(0); |
jjmedina | 0:a5367bd4e404 | 1562 | if( n95Vmax1 >= n05Vmax1 ) |
jjmedina | 0:a5367bd4e404 | 1563 | { |
jjmedina | 0:a5367bd4e404 | 1564 | length_maximo_aA95 = (int)((n95Vmax1 - n05Vmax1)*0.05) + 1; |
jjmedina | 0:a5367bd4e404 | 1565 | }else |
jjmedina | 0:a5367bd4e404 | 1566 | { |
jjmedina | 0:a5367bd4e404 | 1567 | length_maximo_aA95 = 1; |
jjmedina | 0:a5367bd4e404 | 1568 | } |
jjmedina | 0:a5367bd4e404 | 1569 | //int length_maximo_aA95 = (int)((n95Vmax1 - n05Vmax1)*0.05) + 1; |
jjmedina | 0:a5367bd4e404 | 1570 | |
jjmedina | 0:a5367bd4e404 | 1571 | int maxindex_aA95(0); |
jjmedina | 0:a5367bd4e404 | 1572 | vector<float> *maximo_aA95 = new vector<float> (length_maximo_aA95,0.0); |
jjmedina | 0:a5367bd4e404 | 1573 | //vector<float> maximo_aA95 (length_maximo_aA95,0.0); |
jjmedina | 0:a5367bd4e404 | 1574 | vector<float> *diferencia_aA95_vec = new vector<float>; |
jjmedina | 0:a5367bd4e404 | 1575 | vector<float>::iterator up_aA95; |
jjmedina | 0:a5367bd4e404 | 1576 | float diferencia_aA95; |
jjmedina | 0:a5367bd4e404 | 1577 | float maximoaA95; |
jjmedina | 0:a5367bd4e404 | 1578 | int contador_aA95(0); |
jjmedina | 0:a5367bd4e404 | 1579 | //dA95 |
jjmedina | 0:a5367bd4e404 | 1580 | int length_maximo_dA95(0); |
jjmedina | 0:a5367bd4e404 | 1581 | if( n05Vmax2 >= n95Vmax2 ) |
jjmedina | 0:a5367bd4e404 | 1582 | { |
jjmedina | 0:a5367bd4e404 | 1583 | length_maximo_dA95 = (int)((n05Vmax2 - n95Vmax2)*0.05) + 1; |
jjmedina | 0:a5367bd4e404 | 1584 | }else |
jjmedina | 0:a5367bd4e404 | 1585 | { |
jjmedina | 0:a5367bd4e404 | 1586 | length_maximo_dA95 = 1; |
jjmedina | 0:a5367bd4e404 | 1587 | } |
jjmedina | 0:a5367bd4e404 | 1588 | //int length_maximo_dA95 = (int)((n05Vmax2 - n95Vmax2)*0.05) + 1; |
jjmedina | 0:a5367bd4e404 | 1589 | |
jjmedina | 0:a5367bd4e404 | 1590 | int maxindex_dA95(0); |
jjmedina | 0:a5367bd4e404 | 1591 | vector<float> *maximo_dA95 = new vector<float> (length_maximo_dA95,0.0); |
jjmedina | 0:a5367bd4e404 | 1592 | //vector<float> maximo_dA95 (length_maximo_dA95,0.0); |
jjmedina | 0:a5367bd4e404 | 1593 | vector<float> *diferencia_dA95_vec = new vector<float>; |
jjmedina | 0:a5367bd4e404 | 1594 | vector<float>::iterator up_dA95; |
jjmedina | 0:a5367bd4e404 | 1595 | float diferencia_dA95; |
jjmedina | 0:a5367bd4e404 | 1596 | float maximodA95; |
jjmedina | 0:a5367bd4e404 | 1597 | int contador_dA95(0); |
jjmedina | 0:a5367bd4e404 | 1598 | |
jjmedina | 0:a5367bd4e404 | 1599 | //Archivos de lectura y escritura |
jjmedina | 0:a5367bd4e404 | 1600 | |
jjmedina | 0:a5367bd4e404 | 1601 | FILE * fp; |
jjmedina | 0:a5367bd4e404 | 1602 | |
jjmedina | 0:a5367bd4e404 | 1603 | char NoViajel[2]; |
jjmedina | 0:a5367bd4e404 | 1604 | char data[50]; |
jjmedina | 0:a5367bd4e404 | 1605 | |
jjmedina | 0:a5367bd4e404 | 1606 | strcpy(data,"/local/dvaj_"); |
jjmedina | 0:a5367bd4e404 | 1607 | sprintf (NoViajel, "%d", ContadorEnsayos); |
jjmedina | 0:a5367bd4e404 | 1608 | strcat(data,NoViajel); |
jjmedina | 0:a5367bd4e404 | 1609 | strcat(data,".txt"); |
jjmedina | 0:a5367bd4e404 | 1610 | |
jjmedina | 0:a5367bd4e404 | 1611 | fp = fopen(data,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", data); } |
jjmedina | 0:a5367bd4e404 | 1612 | |
jjmedina | 0:a5367bd4e404 | 1613 | int nDatos = (ContadorDatos-(ndes+nmed)); |
jjmedina | 0:a5367bd4e404 | 1614 | if (nDatos < 0) {nDatos = 0;} |
jjmedina | 0:a5367bd4e404 | 1615 | |
jjmedina | 0:a5367bd4e404 | 1616 | |
jjmedina | 0:a5367bd4e404 | 1617 | if ( (maximoAV95 == NULL) || ( maximo_aA95 == NULL) || ( maximo_dA95 == NULL) ) |
jjmedina | 0:a5367bd4e404 | 1618 | { |
jjmedina | 0:a5367bd4e404 | 1619 | display.Clear();display.SetXY(0,0); display.printf("RAM overflow\n"); |
jjmedina | 0:a5367bd4e404 | 1620 | }else |
jjmedina | 0:a5367bd4e404 | 1621 | { |
jjmedina | 0:a5367bd4e404 | 1622 | //loop principal, pasada 1 |
jjmedina | 0:a5367bd4e404 | 1623 | for(unsigned int i = 0; i<(nDatos); i++) |
jjmedina | 0:a5367bd4e404 | 1624 | { |
jjmedina | 0:a5367bd4e404 | 1625 | //read data row |
jjmedina | 0:a5367bd4e404 | 1626 | fscanf(fp,"%f %f %f %f %f", &t, &dZ, &vZ, &aZ, &jerk); |
jjmedina | 0:a5367bd4e404 | 1627 | |
jjmedina | 0:a5367bd4e404 | 1628 | //VA95 |
jjmedina | 0:a5367bd4e404 | 1629 | if( (i <= Bo_veloc_decel) && ( i >= Bo_veloc_acel) ) |
jjmedina | 0:a5367bd4e404 | 1630 | { |
jjmedina | 0:a5367bd4e404 | 1631 | |
jjmedina | 0:a5367bd4e404 | 1632 | //pasada 1 |
jjmedina | 0:a5367bd4e404 | 1633 | for (unsigned int j = 0; j<length_maximoVA95; j++) |
jjmedina | 0:a5367bd4e404 | 1634 | { |
jjmedina | 0:a5367bd4e404 | 1635 | |
jjmedina | 0:a5367bd4e404 | 1636 | diferencia = fabs(vZ) - maximoAV95->at(j); |
jjmedina | 0:a5367bd4e404 | 1637 | if ( diferencia < 0 ) |
jjmedina | 0:a5367bd4e404 | 1638 | { |
jjmedina | 0:a5367bd4e404 | 1639 | diferencia_vec->push_back(0.0); |
jjmedina | 0:a5367bd4e404 | 1640 | contadorA95++; |
jjmedina | 0:a5367bd4e404 | 1641 | }else |
jjmedina | 0:a5367bd4e404 | 1642 | { |
jjmedina | 0:a5367bd4e404 | 1643 | diferencia_vec->push_back(diferencia); |
jjmedina | 0:a5367bd4e404 | 1644 | } |
jjmedina | 0:a5367bd4e404 | 1645 | |
jjmedina | 0:a5367bd4e404 | 1646 | } |
jjmedina | 0:a5367bd4e404 | 1647 | if ( contadorA95 == length_maximoVA95 ){} |
jjmedina | 0:a5367bd4e404 | 1648 | else |
jjmedina | 0:a5367bd4e404 | 1649 | { |
jjmedina | 0:a5367bd4e404 | 1650 | |
jjmedina | 0:a5367bd4e404 | 1651 | maximo = *max_element(diferencia_vec->begin(),diferencia_vec->end()); |
jjmedina | 0:a5367bd4e404 | 1652 | up = find(diferencia_vec->begin(), diferencia_vec->end(), maximo); |
jjmedina | 0:a5367bd4e404 | 1653 | maxindex = int(up - diferencia_vec->begin()); |
jjmedina | 0:a5367bd4e404 | 1654 | maximoAV95->erase(maximoAV95->begin() + maxindex ); |
jjmedina | 0:a5367bd4e404 | 1655 | maximoAV95->insert(maximoAV95->begin() + maxindex ,fabs(vZ)); |
jjmedina | 0:a5367bd4e404 | 1656 | } |
jjmedina | 0:a5367bd4e404 | 1657 | contadorA95 = 0; |
jjmedina | 0:a5367bd4e404 | 1658 | diferencia_vec->clear(); |
jjmedina | 0:a5367bd4e404 | 1659 | } |
jjmedina | 0:a5367bd4e404 | 1660 | |
jjmedina | 0:a5367bd4e404 | 1661 | //aA95 |
jjmedina | 0:a5367bd4e404 | 1662 | if( (i <= n95Vmax1) && ( i >= n05Vmax1) ) |
jjmedina | 0:a5367bd4e404 | 1663 | { |
jjmedina | 0:a5367bd4e404 | 1664 | |
jjmedina | 0:a5367bd4e404 | 1665 | //pasada 1 |
jjmedina | 0:a5367bd4e404 | 1666 | for (unsigned int j = 0; j<length_maximo_aA95; j++) |
jjmedina | 0:a5367bd4e404 | 1667 | { |
jjmedina | 0:a5367bd4e404 | 1668 | |
jjmedina | 0:a5367bd4e404 | 1669 | diferencia_aA95 = fabs(aZ) - maximo_aA95->at(j); |
jjmedina | 0:a5367bd4e404 | 1670 | if ( diferencia_aA95 < 0 ) |
jjmedina | 0:a5367bd4e404 | 1671 | { |
jjmedina | 0:a5367bd4e404 | 1672 | diferencia_aA95_vec->push_back(0.0); |
jjmedina | 0:a5367bd4e404 | 1673 | contador_aA95++; |
jjmedina | 0:a5367bd4e404 | 1674 | }else |
jjmedina | 0:a5367bd4e404 | 1675 | { |
jjmedina | 0:a5367bd4e404 | 1676 | diferencia_aA95_vec->push_back(diferencia_aA95); |
jjmedina | 0:a5367bd4e404 | 1677 | } |
jjmedina | 0:a5367bd4e404 | 1678 | |
jjmedina | 0:a5367bd4e404 | 1679 | } |
jjmedina | 0:a5367bd4e404 | 1680 | if ( contador_aA95 == length_maximo_aA95 ){} |
jjmedina | 0:a5367bd4e404 | 1681 | else |
jjmedina | 0:a5367bd4e404 | 1682 | { |
jjmedina | 0:a5367bd4e404 | 1683 | |
jjmedina | 0:a5367bd4e404 | 1684 | maximoaA95 = *max_element(diferencia_aA95_vec->begin(),diferencia_aA95_vec->end()); |
jjmedina | 0:a5367bd4e404 | 1685 | up_aA95 = find(diferencia_aA95_vec->begin(), diferencia_aA95_vec->end(), maximoaA95); |
jjmedina | 0:a5367bd4e404 | 1686 | maxindex_aA95 = int(up_aA95 - diferencia_aA95_vec->begin()); |
jjmedina | 0:a5367bd4e404 | 1687 | maximo_aA95->erase(maximo_aA95->begin() + maxindex_aA95 ); |
jjmedina | 0:a5367bd4e404 | 1688 | maximo_aA95->insert(maximo_aA95->begin() + maxindex_aA95 ,fabs(aZ)); |
jjmedina | 0:a5367bd4e404 | 1689 | } |
jjmedina | 0:a5367bd4e404 | 1690 | contador_aA95 = 0; |
jjmedina | 0:a5367bd4e404 | 1691 | diferencia_aA95_vec->clear(); |
jjmedina | 0:a5367bd4e404 | 1692 | } |
jjmedina | 0:a5367bd4e404 | 1693 | |
jjmedina | 0:a5367bd4e404 | 1694 | //dA95 |
jjmedina | 0:a5367bd4e404 | 1695 | if( (i <= n05Vmax2) && ( i >= n95Vmax2) ) |
jjmedina | 0:a5367bd4e404 | 1696 | { |
jjmedina | 0:a5367bd4e404 | 1697 | |
jjmedina | 0:a5367bd4e404 | 1698 | //pasada 1 |
jjmedina | 0:a5367bd4e404 | 1699 | for (unsigned int j = 0; j<length_maximo_dA95; j++) |
jjmedina | 0:a5367bd4e404 | 1700 | { |
jjmedina | 0:a5367bd4e404 | 1701 | |
jjmedina | 0:a5367bd4e404 | 1702 | diferencia_dA95 = fabs(aZ) - maximo_dA95->at(j); |
jjmedina | 0:a5367bd4e404 | 1703 | if ( diferencia_dA95 < 0 ) |
jjmedina | 0:a5367bd4e404 | 1704 | { |
jjmedina | 0:a5367bd4e404 | 1705 | diferencia_dA95_vec->push_back(0.0); |
jjmedina | 0:a5367bd4e404 | 1706 | contador_dA95++; |
jjmedina | 0:a5367bd4e404 | 1707 | }else |
jjmedina | 0:a5367bd4e404 | 1708 | { |
jjmedina | 0:a5367bd4e404 | 1709 | diferencia_dA95_vec->push_back(diferencia_dA95); |
jjmedina | 0:a5367bd4e404 | 1710 | } |
jjmedina | 0:a5367bd4e404 | 1711 | |
jjmedina | 0:a5367bd4e404 | 1712 | } |
jjmedina | 0:a5367bd4e404 | 1713 | if ( contador_dA95 == length_maximo_dA95 ){} |
jjmedina | 0:a5367bd4e404 | 1714 | else |
jjmedina | 0:a5367bd4e404 | 1715 | { |
jjmedina | 0:a5367bd4e404 | 1716 | |
jjmedina | 0:a5367bd4e404 | 1717 | maximodA95 = *max_element(diferencia_dA95_vec->begin(),diferencia_dA95_vec->end()); |
jjmedina | 0:a5367bd4e404 | 1718 | up_dA95 = find(diferencia_dA95_vec->begin(), diferencia_dA95_vec->end(), maximodA95); |
jjmedina | 0:a5367bd4e404 | 1719 | maxindex_dA95 = int(up_dA95 - diferencia_dA95_vec->begin()); |
jjmedina | 0:a5367bd4e404 | 1720 | maximo_dA95->erase(maximo_dA95->begin() + maxindex_dA95 ); |
jjmedina | 0:a5367bd4e404 | 1721 | maximo_dA95->insert(maximo_dA95->begin() + maxindex_dA95 ,fabs(aZ)); |
jjmedina | 0:a5367bd4e404 | 1722 | } |
jjmedina | 0:a5367bd4e404 | 1723 | contador_dA95 = 0; |
jjmedina | 0:a5367bd4e404 | 1724 | diferencia_dA95_vec->clear(); |
jjmedina | 0:a5367bd4e404 | 1725 | } |
jjmedina | 0:a5367bd4e404 | 1726 | |
jjmedina | 0:a5367bd4e404 | 1727 | }//Fin loop principal, pasada 1. |
jjmedina | 0:a5367bd4e404 | 1728 | |
jjmedina | 0:a5367bd4e404 | 1729 | sort(maximoAV95->begin(),maximoAV95->end());//ascending |
jjmedina | 0:a5367bd4e404 | 1730 | reverse(maximoAV95->begin(),maximoAV95->end());//descending |
jjmedina | 0:a5367bd4e404 | 1731 | sort(maximo_aA95->begin(),maximo_aA95->end());//ascending |
jjmedina | 0:a5367bd4e404 | 1732 | reverse(maximo_aA95->begin(),maximo_aA95->end());//descending |
jjmedina | 0:a5367bd4e404 | 1733 | sort(maximo_dA95->begin(),maximo_dA95->end());//ascending |
jjmedina | 0:a5367bd4e404 | 1734 | reverse(maximo_dA95->begin(),maximo_dA95->end());//descending |
jjmedina | 0:a5367bd4e404 | 1735 | |
jjmedina | 0:a5367bd4e404 | 1736 | |
jjmedina | 0:a5367bd4e404 | 1737 | //cierra ficheros y variables |
jjmedina | 0:a5367bd4e404 | 1738 | fclose(fp); |
jjmedina | 0:a5367bd4e404 | 1739 | |
jjmedina | 0:a5367bd4e404 | 1740 | //pasada2 |
jjmedina | 0:a5367bd4e404 | 1741 | fp = fopen(data,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", data); } |
jjmedina | 0:a5367bd4e404 | 1742 | |
jjmedina | 0:a5367bd4e404 | 1743 | |
jjmedina | 0:a5367bd4e404 | 1744 | //loop principal, pasada 2 |
jjmedina | 0:a5367bd4e404 | 1745 | double resta(1000.0); |
jjmedina | 0:a5367bd4e404 | 1746 | double resta_aA95(1000.0); |
jjmedina | 0:a5367bd4e404 | 1747 | double resta_dA95(1000.0); |
jjmedina | 0:a5367bd4e404 | 1748 | for(unsigned int i = 0; i<(nDatos); i++) |
jjmedina | 0:a5367bd4e404 | 1749 | { |
jjmedina | 0:a5367bd4e404 | 1750 | //read data row |
jjmedina | 0:a5367bd4e404 | 1751 | fscanf(fp,"%f %f %f %f %f", &t, &dZ, &vZ, &aZ, &jerk); |
jjmedina | 0:a5367bd4e404 | 1752 | |
jjmedina | 0:a5367bd4e404 | 1753 | //VA95 |
jjmedina | 0:a5367bd4e404 | 1754 | |
jjmedina | 0:a5367bd4e404 | 1755 | if( (i <= Bo_veloc_decel) && ( i >= Bo_veloc_acel) ) |
jjmedina | 0:a5367bd4e404 | 1756 | { |
jjmedina | 0:a5367bd4e404 | 1757 | if( ((maximoAV95->at(maximoAV95->size() - 1) - fabs(vZ)) <= resta) && (fabs(vZ) < maximoAV95->at(maximoAV95->size() - 1)) ) |
jjmedina | 0:a5367bd4e404 | 1758 | { |
jjmedina | 0:a5367bd4e404 | 1759 | VA95 = fabs(vZ); |
jjmedina | 0:a5367bd4e404 | 1760 | resta = maximoAV95->at(maximoAV95->size() - 1) - fabs(vZ); |
jjmedina | 0:a5367bd4e404 | 1761 | } |
jjmedina | 0:a5367bd4e404 | 1762 | } |
jjmedina | 0:a5367bd4e404 | 1763 | //aA95 |
jjmedina | 0:a5367bd4e404 | 1764 | |
jjmedina | 0:a5367bd4e404 | 1765 | if( (i <= n95Vmax1) && ( i >= n05Vmax1) ) |
jjmedina | 0:a5367bd4e404 | 1766 | { |
jjmedina | 0:a5367bd4e404 | 1767 | if( ((maximo_aA95->at(maximo_aA95->size() - 1) - fabs(aZ)) <= resta_aA95) && (fabs(aZ) < maximo_aA95->at(maximo_aA95->size() - 1)) ) |
jjmedina | 0:a5367bd4e404 | 1768 | { |
jjmedina | 0:a5367bd4e404 | 1769 | aA95 = fabs(aZ); |
jjmedina | 0:a5367bd4e404 | 1770 | resta_aA95 = maximo_aA95->at(maximo_aA95->size() - 1) - fabs(aZ); |
jjmedina | 0:a5367bd4e404 | 1771 | } |
jjmedina | 0:a5367bd4e404 | 1772 | } |
jjmedina | 0:a5367bd4e404 | 1773 | //dA95 |
jjmedina | 0:a5367bd4e404 | 1774 | |
jjmedina | 0:a5367bd4e404 | 1775 | |
jjmedina | 0:a5367bd4e404 | 1776 | |
jjmedina | 0:a5367bd4e404 | 1777 | if( (i <= n05Vmax2) && ( i >= n95Vmax2) ) |
jjmedina | 0:a5367bd4e404 | 1778 | { |
jjmedina | 0:a5367bd4e404 | 1779 | if( ((maximo_dA95->at(maximo_dA95->size() - 1) - fabs(aZ)) <= resta_dA95) && (fabs(aZ) < maximo_dA95->at(maximo_dA95->size() - 1)) ) |
jjmedina | 0:a5367bd4e404 | 1780 | { |
jjmedina | 0:a5367bd4e404 | 1781 | dA95 = fabs(aZ); |
jjmedina | 0:a5367bd4e404 | 1782 | resta_dA95 = maximo_dA95->at(maximo_dA95->size() - 1) - fabs(aZ); |
jjmedina | 0:a5367bd4e404 | 1783 | } |
jjmedina | 0:a5367bd4e404 | 1784 | } |
jjmedina | 0:a5367bd4e404 | 1785 | |
jjmedina | 0:a5367bd4e404 | 1786 | |
jjmedina | 0:a5367bd4e404 | 1787 | |
jjmedina | 0:a5367bd4e404 | 1788 | } |
jjmedina | 0:a5367bd4e404 | 1789 | //clear all vectors |
jjmedina | 0:a5367bd4e404 | 1790 | maximoAV95->clear(); |
jjmedina | 0:a5367bd4e404 | 1791 | delete maximoAV95; |
jjmedina | 0:a5367bd4e404 | 1792 | diferencia_vec->clear(); |
jjmedina | 0:a5367bd4e404 | 1793 | delete diferencia_vec; |
jjmedina | 0:a5367bd4e404 | 1794 | maximo_aA95->clear(); |
jjmedina | 0:a5367bd4e404 | 1795 | delete maximo_aA95; |
jjmedina | 0:a5367bd4e404 | 1796 | diferencia_aA95_vec->clear(); |
jjmedina | 0:a5367bd4e404 | 1797 | delete diferencia_aA95_vec; |
jjmedina | 0:a5367bd4e404 | 1798 | maximo_dA95->clear(); |
jjmedina | 0:a5367bd4e404 | 1799 | delete maximo_dA95; |
jjmedina | 0:a5367bd4e404 | 1800 | diferencia_dA95_vec->clear(); |
jjmedina | 0:a5367bd4e404 | 1801 | delete diferencia_dA95_vec; |
jjmedina | 0:a5367bd4e404 | 1802 | |
jjmedina | 0:a5367bd4e404 | 1803 | }//fin else |
jjmedina | 0:a5367bd4e404 | 1804 | |
jjmedina | 0:a5367bd4e404 | 1805 | fclose(fp); |
jjmedina | 0:a5367bd4e404 | 1806 | |
jjmedina | 0:a5367bd4e404 | 1807 | } |
jjmedina | 0:a5367bd4e404 | 1808 | |
jjmedina | 0:a5367bd4e404 | 1809 | void vibraReadFilter(unsigned int ContadorEnsayos, unsigned int ContadorDatos) |
jjmedina | 0:a5367bd4e404 | 1810 | { |
jjmedina | 0:a5367bd4e404 | 1811 | |
jjmedina | 0:a5367bd4e404 | 1812 | |
jjmedina | 0:a5367bd4e404 | 1813 | float t; |
jjmedina | 0:a5367bd4e404 | 1814 | float aXYZ[3]; //vector aceleracion en el paso i [aX,aY,aZ] |
jjmedina | 0:a5367bd4e404 | 1815 | float Zvibweighted[3]={0.0,0.0,0.0}; |
jjmedina | 0:a5367bd4e404 | 1816 | float Xvibweighted[3]={0.0,0.0,0.0}; |
jjmedina | 0:a5367bd4e404 | 1817 | float Yvibweighted[3]={0.0,0.0,0.0}; |
jjmedina | 0:a5367bd4e404 | 1818 | float aZ[3], aX[3], aY[3]; |
jjmedina | 0:a5367bd4e404 | 1819 | |
jjmedina | 0:a5367bd4e404 | 1820 | FILE * fp; |
jjmedina | 0:a5367bd4e404 | 1821 | FILE * fp3; |
jjmedina | 0:a5367bd4e404 | 1822 | |
jjmedina | 0:a5367bd4e404 | 1823 | char NoViajel[2]; |
jjmedina | 0:a5367bd4e404 | 1824 | char resultsFile[50]; |
jjmedina | 0:a5367bd4e404 | 1825 | char resultsFileW[50]; |
jjmedina | 0:a5367bd4e404 | 1826 | |
jjmedina | 0:a5367bd4e404 | 1827 | //strcpy(resultsFile,"/local/IV_"); |
jjmedina | 0:a5367bd4e404 | 1828 | strcpy(resultsFile,"/local/raw_V_"); |
jjmedina | 0:a5367bd4e404 | 1829 | |
jjmedina | 0:a5367bd4e404 | 1830 | strcpy(resultsFileW,"/local/W_IV_"); |
jjmedina | 0:a5367bd4e404 | 1831 | |
jjmedina | 0:a5367bd4e404 | 1832 | sprintf (NoViajel, "%d", ContadorEnsayos); |
jjmedina | 0:a5367bd4e404 | 1833 | strcat(resultsFile,NoViajel); strcat(resultsFileW,NoViajel);//fichero con todos los datos de la función anterior IV_ |
jjmedina | 0:a5367bd4e404 | 1834 | strcat(resultsFile,".txt"); strcat(resultsFileW,".txt"); |
jjmedina | 0:a5367bd4e404 | 1835 | |
jjmedina | 0:a5367bd4e404 | 1836 | //fichero con todos los datos de la función anterior IV_ |
jjmedina | 0:a5367bd4e404 | 1837 | fp = fopen(resultsFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", resultsFile); } |
jjmedina | 0:a5367bd4e404 | 1838 | //fichero donde guardaré |
jjmedina | 0:a5367bd4e404 | 1839 | fp3 = fopen(resultsFileW,"w"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", resultsFileW); } |
jjmedina | 0:a5367bd4e404 | 1840 | |
jjmedina | 0:a5367bd4e404 | 1841 | //varias pasadas para isofiltrar ax ay y az. isofilter son 4 filtros, pues 4 pasadas |
jjmedina | 0:a5367bd4e404 | 1842 | |
jjmedina | 0:a5367bd4e404 | 1843 | //Hemos recordado |
jjmedina | 0:a5367bd4e404 | 1844 | |
jjmedina | 0:a5367bd4e404 | 1845 | //loop principal. Pasada 1. Hl (Low Pass 100Hz Orden 2) |
jjmedina | 0:a5367bd4e404 | 1846 | for(unsigned int i = 0; i<(ContadorDatos); i++) |
jjmedina | 0:a5367bd4e404 | 1847 | { |
jjmedina | 0:a5367bd4e404 | 1848 | fscanf(fp,"%f %f %f %f", &t, &aXYZ[0], &aXYZ[1], &aXYZ[2]);//lee tiempo y 3 aceleraciones X, Y, Z |
jjmedina | 0:a5367bd4e404 | 1849 | //restamos gravedad a aZ |
jjmedina | 0:a5367bd4e404 | 1850 | aXYZ[2] = aXYZ[2] - aZm0; |
jjmedina | 0:a5367bd4e404 | 1851 | aXYZ[0] = aXYZ[0] - aXm0; |
jjmedina | 0:a5367bd4e404 | 1852 | aXYZ[1] = aXYZ[1] - aYm0; |
jjmedina | 0:a5367bd4e404 | 1853 | //isofilter |
jjmedina | 0:a5367bd4e404 | 1854 | |
jjmedina | 0:a5367bd4e404 | 1855 | int i2 = i-(ndes+nmed); //Nuevo idice para calcular posicion y velocidad |
jjmedina | 0:a5367bd4e404 | 1856 | |
jjmedina | 0:a5367bd4e404 | 1857 | |
jjmedina | 0:a5367bd4e404 | 1858 | if ( i==0 ) |
jjmedina | 0:a5367bd4e404 | 1859 | { |
jjmedina | 0:a5367bd4e404 | 1860 | Zvibweighted[0] = aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 1861 | Xvibweighted[0] = aXYZ[0]; |
jjmedina | 0:a5367bd4e404 | 1862 | Yvibweighted[0] = aXYZ[1]; |
jjmedina | 0:a5367bd4e404 | 1863 | aZ[0] =aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 1864 | aX[0] =aXYZ[0]; |
jjmedina | 0:a5367bd4e404 | 1865 | aY[0] =aXYZ[1]; |
jjmedina | 0:a5367bd4e404 | 1866 | |
jjmedina | 0:a5367bd4e404 | 1867 | |
jjmedina | 0:a5367bd4e404 | 1868 | // fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[0], Yvibweighted[0], Zvibweighted[0]); |
jjmedina | 0:a5367bd4e404 | 1869 | |
jjmedina | 0:a5367bd4e404 | 1870 | }else if ( i==1 ) |
jjmedina | 0:a5367bd4e404 | 1871 | { |
jjmedina | 0:a5367bd4e404 | 1872 | Zvibweighted[1] = aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 1873 | Xvibweighted[1] = aXYZ[0]; |
jjmedina | 0:a5367bd4e404 | 1874 | Yvibweighted[1] = aXYZ[1]; |
jjmedina | 0:a5367bd4e404 | 1875 | aZ[1] =aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 1876 | aX[1] =aXYZ[0]; |
jjmedina | 0:a5367bd4e404 | 1877 | aY[1] =aXYZ[1]; |
jjmedina | 0:a5367bd4e404 | 1878 | |
jjmedina | 0:a5367bd4e404 | 1879 | // fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[1], Yvibweighted[1], Zvibweighted[1]); |
jjmedina | 0:a5367bd4e404 | 1880 | |
jjmedina | 0:a5367bd4e404 | 1881 | }else |
jjmedina | 0:a5367bd4e404 | 1882 | { |
jjmedina | 0:a5367bd4e404 | 1883 | |
jjmedina | 0:a5367bd4e404 | 1884 | aZ[2] = aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 1885 | aX[2] = aXYZ[0]; |
jjmedina | 0:a5367bd4e404 | 1886 | aY[2] = aXYZ[1]; |
jjmedina | 0:a5367bd4e404 | 1887 | |
jjmedina | 0:a5367bd4e404 | 1888 | Zvibweighted[2] = isoFilter_Wk_z_Hl(Zvibweighted[0],Zvibweighted[1],aZ, PasoTiempo); |
jjmedina | 0:a5367bd4e404 | 1889 | Xvibweighted[2] = isoFilter_Wd_xy_Hl(Xvibweighted[0],Xvibweighted[1],aX, PasoTiempo); |
jjmedina | 0:a5367bd4e404 | 1890 | Yvibweighted[2] = isoFilter_Wd_xy_Hl(Yvibweighted[0],Yvibweighted[1],aY, PasoTiempo); |
jjmedina | 0:a5367bd4e404 | 1891 | |
jjmedina | 0:a5367bd4e404 | 1892 | if(i2 >= 0) |
jjmedina | 0:a5367bd4e404 | 1893 | { |
jjmedina | 0:a5367bd4e404 | 1894 | fprintf(fp3,"%f %f %f %f \n", (t), Xvibweighted[2], Yvibweighted[2], Zvibweighted[2]); |
jjmedina | 0:a5367bd4e404 | 1895 | } |
jjmedina | 0:a5367bd4e404 | 1896 | |
jjmedina | 0:a5367bd4e404 | 1897 | //updates |
jjmedina | 0:a5367bd4e404 | 1898 | aZ[0] = aZ[1]; |
jjmedina | 0:a5367bd4e404 | 1899 | aZ[1] = aZ[2]; |
jjmedina | 0:a5367bd4e404 | 1900 | aX[0] = aX[1]; |
jjmedina | 0:a5367bd4e404 | 1901 | aX[1] = aX[2]; |
jjmedina | 0:a5367bd4e404 | 1902 | aY[0] = aY[1]; |
jjmedina | 0:a5367bd4e404 | 1903 | aY[1] = aY[2]; |
jjmedina | 0:a5367bd4e404 | 1904 | Zvibweighted[0] = Zvibweighted[1]; |
jjmedina | 0:a5367bd4e404 | 1905 | Zvibweighted[1] = Zvibweighted[2]; |
jjmedina | 0:a5367bd4e404 | 1906 | Xvibweighted[0] = Xvibweighted[1]; |
jjmedina | 0:a5367bd4e404 | 1907 | Xvibweighted[1] = Xvibweighted[2]; |
jjmedina | 0:a5367bd4e404 | 1908 | Yvibweighted[0] = Yvibweighted[1]; |
jjmedina | 0:a5367bd4e404 | 1909 | Yvibweighted[1] = Yvibweighted[2]; |
jjmedina | 0:a5367bd4e404 | 1910 | //azfilter1Hz |
jjmedina | 0:a5367bd4e404 | 1911 | |
jjmedina | 0:a5367bd4e404 | 1912 | } |
jjmedina | 0:a5367bd4e404 | 1913 | |
jjmedina | 0:a5367bd4e404 | 1914 | }//Fin Hl |
jjmedina | 0:a5367bd4e404 | 1915 | fclose(fp); |
jjmedina | 0:a5367bd4e404 | 1916 | fclose(fp3); |
jjmedina | 0:a5367bd4e404 | 1917 | |
jjmedina | 0:a5367bd4e404 | 1918 | |
jjmedina | 0:a5367bd4e404 | 1919 | |
jjmedina | 0:a5367bd4e404 | 1920 | |
jjmedina | 0:a5367bd4e404 | 1921 | ///////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 1922 | //Segund pasada Hh. (High Pass 0.4Hz, Orden 2) |
jjmedina | 0:a5367bd4e404 | 1923 | strcpy(resultsFile,"/local/W_IV_"); |
jjmedina | 0:a5367bd4e404 | 1924 | strcpy(resultsFileW,"/local/W2_IV_"); |
jjmedina | 0:a5367bd4e404 | 1925 | |
jjmedina | 0:a5367bd4e404 | 1926 | sprintf (NoViajel, "%d", ContadorEnsayos); |
jjmedina | 0:a5367bd4e404 | 1927 | strcat(resultsFile,NoViajel); |
jjmedina | 0:a5367bd4e404 | 1928 | strcat(resultsFileW,NoViajel); |
jjmedina | 0:a5367bd4e404 | 1929 | strcat(resultsFile,".txt"); |
jjmedina | 0:a5367bd4e404 | 1930 | strcat(resultsFileW,".txt"); |
jjmedina | 0:a5367bd4e404 | 1931 | |
jjmedina | 0:a5367bd4e404 | 1932 | //fichero con todos los datos de la función anterior IV_ |
jjmedina | 0:a5367bd4e404 | 1933 | fp = fopen(resultsFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", resultsFile); } |
jjmedina | 0:a5367bd4e404 | 1934 | //fichero donde guardaré |
jjmedina | 0:a5367bd4e404 | 1935 | fp3 = fopen(resultsFileW,"w"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", resultsFileW); } |
jjmedina | 0:a5367bd4e404 | 1936 | |
jjmedina | 0:a5367bd4e404 | 1937 | |
jjmedina | 0:a5367bd4e404 | 1938 | |
jjmedina | 0:a5367bd4e404 | 1939 | //loop principal. Pasada 2. Hh (High Pass 0.4Hz Orden 2) |
jjmedina | 0:a5367bd4e404 | 1940 | for(unsigned int i = 0; i<(ContadorDatos-(ndes+nmed)); i++) |
jjmedina | 0:a5367bd4e404 | 1941 | { |
jjmedina | 0:a5367bd4e404 | 1942 | 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. |
jjmedina | 0:a5367bd4e404 | 1943 | |
jjmedina | 0:a5367bd4e404 | 1944 | //isofilter |
jjmedina | 0:a5367bd4e404 | 1945 | |
jjmedina | 0:a5367bd4e404 | 1946 | if ( i==0 ) |
jjmedina | 0:a5367bd4e404 | 1947 | { |
jjmedina | 0:a5367bd4e404 | 1948 | Zvibweighted[0] = aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 1949 | Xvibweighted[0] = aXYZ[0]; |
jjmedina | 0:a5367bd4e404 | 1950 | Yvibweighted[0] = aXYZ[1]; |
jjmedina | 0:a5367bd4e404 | 1951 | aZ[0] =aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 1952 | aX[0] =aXYZ[0]; |
jjmedina | 0:a5367bd4e404 | 1953 | aY[0] =aXYZ[1]; |
jjmedina | 0:a5367bd4e404 | 1954 | fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[0], Yvibweighted[0], Zvibweighted[0]); |
jjmedina | 0:a5367bd4e404 | 1955 | |
jjmedina | 0:a5367bd4e404 | 1956 | }else if ( i==1 ) |
jjmedina | 0:a5367bd4e404 | 1957 | { |
jjmedina | 0:a5367bd4e404 | 1958 | Zvibweighted[1] = aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 1959 | Xvibweighted[1] = aXYZ[0]; |
jjmedina | 0:a5367bd4e404 | 1960 | Yvibweighted[1] = aXYZ[1]; |
jjmedina | 0:a5367bd4e404 | 1961 | aZ[1] =aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 1962 | aX[1] =aXYZ[0]; |
jjmedina | 0:a5367bd4e404 | 1963 | aY[1] =aXYZ[1]; |
jjmedina | 0:a5367bd4e404 | 1964 | fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[1], Yvibweighted[1], Zvibweighted[1]); |
jjmedina | 0:a5367bd4e404 | 1965 | |
jjmedina | 0:a5367bd4e404 | 1966 | }else |
jjmedina | 0:a5367bd4e404 | 1967 | { |
jjmedina | 0:a5367bd4e404 | 1968 | |
jjmedina | 0:a5367bd4e404 | 1969 | aZ[2] = aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 1970 | aX[2] = aXYZ[0]; |
jjmedina | 0:a5367bd4e404 | 1971 | aY[2] = aXYZ[1]; |
jjmedina | 0:a5367bd4e404 | 1972 | |
jjmedina | 0:a5367bd4e404 | 1973 | // Zvibweighted[2] = aXYZ[2];//isoFilter_Wk_z_Hh(Zvibweighted[0],Zvibweighted[1],aZ); |
jjmedina | 0:a5367bd4e404 | 1974 | // Xvibweighted[2] = aXYZ[0];//isoFilter_Wd_xy_Hh(Xvibweighted[0],Xvibweighted[1],aX); //OJO: el filtro High Pass no funciona bien |
jjmedina | 0:a5367bd4e404 | 1975 | // Yvibweighted[2] = aXYZ[1];//isoFilter_Wd_xy_Hh(Yvibweighted[0],Yvibweighted[1],aY); |
jjmedina | 0:a5367bd4e404 | 1976 | Zvibweighted[2] = isoFilter_Wk_z_Hh(Zvibweighted[0],Zvibweighted[1],aZ, PasoTiempo); |
jjmedina | 0:a5367bd4e404 | 1977 | Xvibweighted[2] = isoFilter_Wd_xy_Hh(Xvibweighted[0],Xvibweighted[1],aX, PasoTiempo); |
jjmedina | 0:a5367bd4e404 | 1978 | Yvibweighted[2] = isoFilter_Wd_xy_Hh(Yvibweighted[0],Yvibweighted[1],aY, PasoTiempo); |
jjmedina | 0:a5367bd4e404 | 1979 | fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[2], Yvibweighted[2], Zvibweighted[2]); |
jjmedina | 0:a5367bd4e404 | 1980 | //updates |
jjmedina | 0:a5367bd4e404 | 1981 | aZ[0] = aZ[1]; |
jjmedina | 0:a5367bd4e404 | 1982 | aZ[1] = aZ[2]; |
jjmedina | 0:a5367bd4e404 | 1983 | aX[0] = aX[1]; |
jjmedina | 0:a5367bd4e404 | 1984 | aX[1] = aX[2]; |
jjmedina | 0:a5367bd4e404 | 1985 | aY[0] = aY[1]; |
jjmedina | 0:a5367bd4e404 | 1986 | aY[1] = aY[2]; |
jjmedina | 0:a5367bd4e404 | 1987 | Zvibweighted[0] = Zvibweighted[1]; |
jjmedina | 0:a5367bd4e404 | 1988 | Zvibweighted[1] = Zvibweighted[2]; |
jjmedina | 0:a5367bd4e404 | 1989 | Xvibweighted[0] = Xvibweighted[1]; |
jjmedina | 0:a5367bd4e404 | 1990 | Xvibweighted[1] = Xvibweighted[2]; |
jjmedina | 0:a5367bd4e404 | 1991 | Yvibweighted[0] = Yvibweighted[1]; |
jjmedina | 0:a5367bd4e404 | 1992 | Yvibweighted[1] = Yvibweighted[2]; |
jjmedina | 0:a5367bd4e404 | 1993 | } |
jjmedina | 0:a5367bd4e404 | 1994 | |
jjmedina | 0:a5367bd4e404 | 1995 | }//Fin Hh1 |
jjmedina | 0:a5367bd4e404 | 1996 | fclose(fp); |
jjmedina | 0:a5367bd4e404 | 1997 | fclose(fp3); |
jjmedina | 0:a5367bd4e404 | 1998 | int rmDVA0 = remove(resultsFile); |
jjmedina | 0:a5367bd4e404 | 1999 | |
jjmedina | 0:a5367bd4e404 | 2000 | |
jjmedina | 0:a5367bd4e404 | 2001 | ///////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 2002 | //Tercera pasada Ht. (Ht filtro transicion) |
jjmedina | 0:a5367bd4e404 | 2003 | strcpy(resultsFile,"/local/W2_IV_"); |
jjmedina | 0:a5367bd4e404 | 2004 | strcpy(resultsFileW,"/local/W4_IV_"); |
jjmedina | 0:a5367bd4e404 | 2005 | |
jjmedina | 0:a5367bd4e404 | 2006 | sprintf (NoViajel, "%d", ContadorEnsayos); |
jjmedina | 0:a5367bd4e404 | 2007 | strcat(resultsFile,NoViajel); |
jjmedina | 0:a5367bd4e404 | 2008 | strcat(resultsFileW,NoViajel); |
jjmedina | 0:a5367bd4e404 | 2009 | strcat(resultsFile,".txt"); |
jjmedina | 0:a5367bd4e404 | 2010 | strcat(resultsFileW,".txt"); |
jjmedina | 0:a5367bd4e404 | 2011 | |
jjmedina | 0:a5367bd4e404 | 2012 | //fichero con todos los datos de la función anterior IV_ |
jjmedina | 0:a5367bd4e404 | 2013 | fp = fopen(resultsFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", resultsFile); } |
jjmedina | 0:a5367bd4e404 | 2014 | //fichero donde guardaré |
jjmedina | 0:a5367bd4e404 | 2015 | fp3 = fopen(resultsFileW,"w"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", resultsFileW); } |
jjmedina | 0:a5367bd4e404 | 2016 | |
jjmedina | 0:a5367bd4e404 | 2017 | |
jjmedina | 0:a5367bd4e404 | 2018 | |
jjmedina | 0:a5367bd4e404 | 2019 | |
jjmedina | 0:a5367bd4e404 | 2020 | //loop principal. Pasada 3. Ht (Transicion) |
jjmedina | 0:a5367bd4e404 | 2021 | for(unsigned int i = 0; i<(ContadorDatos-(ndes+nmed)); i++) |
jjmedina | 0:a5367bd4e404 | 2022 | { |
jjmedina | 0:a5367bd4e404 | 2023 | 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. |
jjmedina | 0:a5367bd4e404 | 2024 | |
jjmedina | 0:a5367bd4e404 | 2025 | //isofilter |
jjmedina | 0:a5367bd4e404 | 2026 | |
jjmedina | 0:a5367bd4e404 | 2027 | if ( i==0 ) |
jjmedina | 0:a5367bd4e404 | 2028 | { |
jjmedina | 0:a5367bd4e404 | 2029 | Zvibweighted[0] = aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 2030 | Xvibweighted[0] = aXYZ[0]; |
jjmedina | 0:a5367bd4e404 | 2031 | Yvibweighted[0] = aXYZ[1]; |
jjmedina | 0:a5367bd4e404 | 2032 | aZ[0] =aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 2033 | aX[0] =aXYZ[0]; |
jjmedina | 0:a5367bd4e404 | 2034 | aY[0] =aXYZ[1]; |
jjmedina | 0:a5367bd4e404 | 2035 | fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[0], Yvibweighted[0], Zvibweighted[0]); |
jjmedina | 0:a5367bd4e404 | 2036 | |
jjmedina | 0:a5367bd4e404 | 2037 | }else if ( i==1 ) |
jjmedina | 0:a5367bd4e404 | 2038 | { |
jjmedina | 0:a5367bd4e404 | 2039 | Zvibweighted[1] = aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 2040 | Xvibweighted[1] = aXYZ[0]; |
jjmedina | 0:a5367bd4e404 | 2041 | Yvibweighted[1] = aXYZ[1]; |
jjmedina | 0:a5367bd4e404 | 2042 | aZ[1] =aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 2043 | aX[1] =aXYZ[0]; |
jjmedina | 0:a5367bd4e404 | 2044 | aY[1] =aXYZ[1]; |
jjmedina | 0:a5367bd4e404 | 2045 | fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[1], Yvibweighted[1], Zvibweighted[1]); |
jjmedina | 0:a5367bd4e404 | 2046 | |
jjmedina | 0:a5367bd4e404 | 2047 | }else |
jjmedina | 0:a5367bd4e404 | 2048 | { |
jjmedina | 0:a5367bd4e404 | 2049 | |
jjmedina | 0:a5367bd4e404 | 2050 | aZ[2] = aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 2051 | aX[2] = aXYZ[0]; |
jjmedina | 0:a5367bd4e404 | 2052 | aY[2] = aXYZ[1]; |
jjmedina | 0:a5367bd4e404 | 2053 | |
jjmedina | 0:a5367bd4e404 | 2054 | Zvibweighted[2] = isoFilter_Wk_z_Ht(Zvibweighted[0],Zvibweighted[1],aZ, PasoTiempo); |
jjmedina | 0:a5367bd4e404 | 2055 | Xvibweighted[2] = isoFilter_Wd_xy_Ht(Xvibweighted[0],Xvibweighted[1],aX, PasoTiempo); |
jjmedina | 0:a5367bd4e404 | 2056 | Yvibweighted[2] = isoFilter_Wd_xy_Ht(Yvibweighted[0],Yvibweighted[1],aY, PasoTiempo); |
jjmedina | 0:a5367bd4e404 | 2057 | // Zvibweighted[2] = aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 2058 | // Xvibweighted[2] = aXYZ[0]; |
jjmedina | 0:a5367bd4e404 | 2059 | // Yvibweighted[2] = aXYZ[1]; |
jjmedina | 0:a5367bd4e404 | 2060 | fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[2], Yvibweighted[2], Zvibweighted[2]); |
jjmedina | 0:a5367bd4e404 | 2061 | //updates |
jjmedina | 0:a5367bd4e404 | 2062 | aZ[0] = aZ[1]; |
jjmedina | 0:a5367bd4e404 | 2063 | aZ[1] = aZ[2]; |
jjmedina | 0:a5367bd4e404 | 2064 | aX[0] = aX[1]; |
jjmedina | 0:a5367bd4e404 | 2065 | aX[1] = aX[2]; |
jjmedina | 0:a5367bd4e404 | 2066 | aY[0] = aY[1]; |
jjmedina | 0:a5367bd4e404 | 2067 | aY[1] = aY[2]; |
jjmedina | 0:a5367bd4e404 | 2068 | Zvibweighted[0] = Zvibweighted[1]; |
jjmedina | 0:a5367bd4e404 | 2069 | Zvibweighted[1] = Zvibweighted[2]; |
jjmedina | 0:a5367bd4e404 | 2070 | Xvibweighted[0] = Xvibweighted[1]; |
jjmedina | 0:a5367bd4e404 | 2071 | Xvibweighted[1] = Xvibweighted[2]; |
jjmedina | 0:a5367bd4e404 | 2072 | Yvibweighted[0] = Yvibweighted[1]; |
jjmedina | 0:a5367bd4e404 | 2073 | Yvibweighted[1] = Yvibweighted[2]; |
jjmedina | 0:a5367bd4e404 | 2074 | } |
jjmedina | 0:a5367bd4e404 | 2075 | |
jjmedina | 0:a5367bd4e404 | 2076 | }//Fin Ht |
jjmedina | 0:a5367bd4e404 | 2077 | fclose(fp); |
jjmedina | 0:a5367bd4e404 | 2078 | fclose(fp3); |
jjmedina | 0:a5367bd4e404 | 2079 | rmDVA0 = remove(resultsFile); |
jjmedina | 0:a5367bd4e404 | 2080 | |
jjmedina | 0:a5367bd4e404 | 2081 | ///////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 2082 | //Cuarta pasada Hs. (Hs filtro subida de escala) |
jjmedina | 0:a5367bd4e404 | 2083 | strcpy(resultsFile,"/local/W4_IV_"); |
jjmedina | 0:a5367bd4e404 | 2084 | strcpy(resultsFileW,"/local/W5_IV_"); |
jjmedina | 0:a5367bd4e404 | 2085 | |
jjmedina | 0:a5367bd4e404 | 2086 | sprintf (NoViajel, "%d", ContadorEnsayos); |
jjmedina | 0:a5367bd4e404 | 2087 | strcat(resultsFile,NoViajel); |
jjmedina | 0:a5367bd4e404 | 2088 | strcat(resultsFileW,NoViajel); |
jjmedina | 0:a5367bd4e404 | 2089 | strcat(resultsFile,".txt"); |
jjmedina | 0:a5367bd4e404 | 2090 | strcat(resultsFileW,".txt"); |
jjmedina | 0:a5367bd4e404 | 2091 | |
jjmedina | 0:a5367bd4e404 | 2092 | //fichero con todos los datos de la función anterior IV_ |
jjmedina | 0:a5367bd4e404 | 2093 | fp = fopen(resultsFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", resultsFile); } |
jjmedina | 0:a5367bd4e404 | 2094 | //fichero donde guardaré |
jjmedina | 0:a5367bd4e404 | 2095 | fp3 = fopen(resultsFileW,"w"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", resultsFileW); } |
jjmedina | 0:a5367bd4e404 | 2096 | |
jjmedina | 0:a5367bd4e404 | 2097 | //loop principal. Pasada 4. Hs (subida escala) |
jjmedina | 0:a5367bd4e404 | 2098 | |
jjmedina | 0:a5367bd4e404 | 2099 | |
jjmedina | 0:a5367bd4e404 | 2100 | |
jjmedina | 0:a5367bd4e404 | 2101 | |
jjmedina | 0:a5367bd4e404 | 2102 | for(unsigned int i = 0; i<(ContadorDatos-(ndes+nmed)); i++) |
jjmedina | 0:a5367bd4e404 | 2103 | { |
jjmedina | 0:a5367bd4e404 | 2104 | 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. |
jjmedina | 0:a5367bd4e404 | 2105 | |
jjmedina | 0:a5367bd4e404 | 2106 | //isofilter |
jjmedina | 0:a5367bd4e404 | 2107 | |
jjmedina | 0:a5367bd4e404 | 2108 | if ( i==0 ) |
jjmedina | 0:a5367bd4e404 | 2109 | { |
jjmedina | 0:a5367bd4e404 | 2110 | Zvibweighted[0] = aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 2111 | Xvibweighted[0] = aXYZ[0]; |
jjmedina | 0:a5367bd4e404 | 2112 | Yvibweighted[0] = aXYZ[1]; |
jjmedina | 0:a5367bd4e404 | 2113 | aZ[0] =aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 2114 | aX[0] =aXYZ[0]; |
jjmedina | 0:a5367bd4e404 | 2115 | aY[0] =aXYZ[1]; |
jjmedina | 0:a5367bd4e404 | 2116 | |
jjmedina | 0:a5367bd4e404 | 2117 | fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[0], Yvibweighted[0], Zvibweighted[0]); |
jjmedina | 0:a5367bd4e404 | 2118 | |
jjmedina | 0:a5367bd4e404 | 2119 | }else if ( i==1 ) |
jjmedina | 0:a5367bd4e404 | 2120 | { |
jjmedina | 0:a5367bd4e404 | 2121 | Zvibweighted[1] = aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 2122 | Xvibweighted[1] = aXYZ[0]; |
jjmedina | 0:a5367bd4e404 | 2123 | Yvibweighted[1] = aXYZ[1]; |
jjmedina | 0:a5367bd4e404 | 2124 | aZ[1] =aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 2125 | aX[1] =aXYZ[0]; |
jjmedina | 0:a5367bd4e404 | 2126 | aY[1] =aXYZ[1]; |
jjmedina | 0:a5367bd4e404 | 2127 | |
jjmedina | 0:a5367bd4e404 | 2128 | fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[1], Yvibweighted[1], Zvibweighted[1]); |
jjmedina | 0:a5367bd4e404 | 2129 | |
jjmedina | 0:a5367bd4e404 | 2130 | }else |
jjmedina | 0:a5367bd4e404 | 2131 | { |
jjmedina | 0:a5367bd4e404 | 2132 | |
jjmedina | 0:a5367bd4e404 | 2133 | aZ[2] = aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 2134 | aX[2] = aXYZ[0]; |
jjmedina | 0:a5367bd4e404 | 2135 | aY[2] = aXYZ[1]; |
jjmedina | 0:a5367bd4e404 | 2136 | |
jjmedina | 0:a5367bd4e404 | 2137 | Zvibweighted[2] = isoFilter_Wk_z_Hs(Zvibweighted[0],Zvibweighted[1],aZ, PasoTiempo); |
jjmedina | 0:a5367bd4e404 | 2138 | Xvibweighted[2] = aX[2]; |
jjmedina | 0:a5367bd4e404 | 2139 | Yvibweighted[2] = aY[2]; |
jjmedina | 0:a5367bd4e404 | 2140 | // Zvibweighted[2] = aXYZ[2]; |
jjmedina | 0:a5367bd4e404 | 2141 | // Xvibweighted[2] = aX[2]; |
jjmedina | 0:a5367bd4e404 | 2142 | // Yvibweighted[2] = aY[2]; |
jjmedina | 0:a5367bd4e404 | 2143 | |
jjmedina | 0:a5367bd4e404 | 2144 | fprintf(fp3,"%f %f %f %f \n", t, Xvibweighted[2], Yvibweighted[2], Zvibweighted[2]); |
jjmedina | 0:a5367bd4e404 | 2145 | //updates |
jjmedina | 0:a5367bd4e404 | 2146 | aZ[0] = aZ[1]; |
jjmedina | 0:a5367bd4e404 | 2147 | aZ[1] = aZ[2]; |
jjmedina | 0:a5367bd4e404 | 2148 | aX[0] = aX[1]; |
jjmedina | 0:a5367bd4e404 | 2149 | aX[1] = aX[2]; |
jjmedina | 0:a5367bd4e404 | 2150 | aY[0] = aY[1]; |
jjmedina | 0:a5367bd4e404 | 2151 | aY[1] = aY[2]; |
jjmedina | 0:a5367bd4e404 | 2152 | Zvibweighted[0] = Zvibweighted[1]; |
jjmedina | 0:a5367bd4e404 | 2153 | Zvibweighted[1] = Zvibweighted[2]; |
jjmedina | 0:a5367bd4e404 | 2154 | Xvibweighted[0] = Xvibweighted[1]; |
jjmedina | 0:a5367bd4e404 | 2155 | Xvibweighted[1] = Xvibweighted[2]; |
jjmedina | 0:a5367bd4e404 | 2156 | Yvibweighted[0] = Yvibweighted[1]; |
jjmedina | 0:a5367bd4e404 | 2157 | Yvibweighted[1] = Yvibweighted[2]; |
jjmedina | 0:a5367bd4e404 | 2158 | |
jjmedina | 0:a5367bd4e404 | 2159 | } |
jjmedina | 0:a5367bd4e404 | 2160 | |
jjmedina | 0:a5367bd4e404 | 2161 | }//Fin Ht |
jjmedina | 0:a5367bd4e404 | 2162 | fclose(fp); |
jjmedina | 0:a5367bd4e404 | 2163 | fclose(fp3); |
jjmedina | 0:a5367bd4e404 | 2164 | rmDVA0 = remove(resultsFile); |
jjmedina | 0:a5367bd4e404 | 2165 | } |
jjmedina | 0:a5367bd4e404 | 2166 | |
jjmedina | 0:a5367bd4e404 | 2167 | |
jjmedina | 0:a5367bd4e404 | 2168 | void calculaStep2(unsigned int ContadorEnsayos, unsigned int ContadorDatos) |
jjmedina | 0:a5367bd4e404 | 2169 | { |
jjmedina | 0:a5367bd4e404 | 2170 | //calcula step2 previo filtadro deaZ ya filtrada a 10Hz, a 1Hz esta vez. |
jjmedina | 0:a5367bd4e404 | 2171 | float t; |
jjmedina | 0:a5367bd4e404 | 2172 | float dZ,vZ,aZfilter10Hz,jerk,aZ[3]; |
jjmedina | 0:a5367bd4e404 | 2173 | float azfilter1Hz[3]; |
jjmedina | 0:a5367bd4e404 | 2174 | //Calculo de step2 |
jjmedina | 0:a5367bd4e404 | 2175 | const float running_time = 1.0; |
jjmedina | 0:a5367bd4e404 | 2176 | |
jjmedina | 0:a5367bd4e404 | 2177 | 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 |
jjmedina | 0:a5367bd4e404 | 2178 | |
jjmedina | 0:a5367bd4e404 | 2179 | float azfilter1Hz_1s[2*Ai+1]; //Aceleraciones dentro del periodo de 0.5 segundos para hacer regresion lineal |
jjmedina | 0:a5367bd4e404 | 2180 | float step2_05s[Ai+1]; //Jerks dentro del periodo de 0.25 |
jjmedina | 0:a5367bd4e404 | 2181 | for (unsigned int i = 0; i<(2*Ai+1); i++) { azfilter1Hz_1s[i] = 0.0; } |
jjmedina | 0:a5367bd4e404 | 2182 | |
jjmedina | 0:a5367bd4e404 | 2183 | for (unsigned int i = 0; i<(Ai+1); i++) { step2_05s[i] = 0.0; } |
jjmedina | 0:a5367bd4e404 | 2184 | |
jjmedina | 0:a5367bd4e404 | 2185 | //Archivos de lectura y escritura |
jjmedina | 0:a5367bd4e404 | 2186 | |
jjmedina | 0:a5367bd4e404 | 2187 | FILE * fp; |
jjmedina | 0:a5367bd4e404 | 2188 | FILE * fp2; |
jjmedina | 0:a5367bd4e404 | 2189 | |
jjmedina | 0:a5367bd4e404 | 2190 | char NoViajel[2]; |
jjmedina | 0:a5367bd4e404 | 2191 | char dispVelFile[50]; char velMod[50]; |
jjmedina | 0:a5367bd4e404 | 2192 | |
jjmedina | 0:a5367bd4e404 | 2193 | strcpy(dispVelFile,"/local/dvaj_"); |
jjmedina | 0:a5367bd4e404 | 2194 | strcpy(velMod,"/local/dvaja_"); |
jjmedina | 0:a5367bd4e404 | 2195 | |
jjmedina | 0:a5367bd4e404 | 2196 | sprintf (NoViajel, "%d", ContadorEnsayos); |
jjmedina | 0:a5367bd4e404 | 2197 | strcat(dispVelFile,NoViajel); strcat(velMod,NoViajel); |
jjmedina | 0:a5367bd4e404 | 2198 | strcat(dispVelFile,".txt"); strcat(velMod,".txt"); |
jjmedina | 0:a5367bd4e404 | 2199 | |
jjmedina | 0:a5367bd4e404 | 2200 | fp = fopen(dispVelFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", dispVelFile); } |
jjmedina | 0:a5367bd4e404 | 2201 | fp2 = fopen(velMod,"w"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", velMod); } |
jjmedina | 0:a5367bd4e404 | 2202 | |
jjmedina | 0:a5367bd4e404 | 2203 | //const float fs = (1000000.0/PasoTiempo);//frecuencia de muestreo debe valer 320 |
jjmedina | 0:a5367bd4e404 | 2204 | |
jjmedina | 0:a5367bd4e404 | 2205 | //Primera pasada. calcula azfilter1Hz |
jjmedina | 0:a5367bd4e404 | 2206 | |
jjmedina | 0:a5367bd4e404 | 2207 | int nDatos = (ContadorDatos-(ndes+nmed)); |
jjmedina | 0:a5367bd4e404 | 2208 | if (nDatos < 0) {nDatos = 0;} |
jjmedina | 0:a5367bd4e404 | 2209 | |
jjmedina | 0:a5367bd4e404 | 2210 | for(unsigned int i = 0; i<nDatos; i++) |
jjmedina | 0:a5367bd4e404 | 2211 | { |
jjmedina | 0:a5367bd4e404 | 2212 | //fscanf(fp,"%f %f %f %f %f %f", &t, &dZ, &vZ, &aZ, &aX, &aY);//aZ filtered, aX y aY raw. |
jjmedina | 0:a5367bd4e404 | 2213 | fscanf(fp,"%f %f %f %f %f", &t, &dZ, &vZ, &aZfilter10Hz, &jerk); |
jjmedina | 0:a5367bd4e404 | 2214 | // |
jjmedina | 0:a5367bd4e404 | 2215 | //isofilter |
jjmedina | 0:a5367bd4e404 | 2216 | if ( i==0 ) |
jjmedina | 0:a5367bd4e404 | 2217 | { |
jjmedina | 0:a5367bd4e404 | 2218 | //azfilter1Hz |
jjmedina | 0:a5367bd4e404 | 2219 | aZ[0] = aZfilter10Hz; |
jjmedina | 0:a5367bd4e404 | 2220 | azfilter1Hz[0] = aZfilter10Hz; |
jjmedina | 0:a5367bd4e404 | 2221 | fprintf(fp2,"%f %f %f %f %f %f \n", t, dZ, vZ, aZfilter10Hz, jerk, azfilter1Hz[0]); |
jjmedina | 0:a5367bd4e404 | 2222 | |
jjmedina | 0:a5367bd4e404 | 2223 | }else if ( i==1 ) |
jjmedina | 0:a5367bd4e404 | 2224 | { |
jjmedina | 0:a5367bd4e404 | 2225 | //azfilter1Hz |
jjmedina | 0:a5367bd4e404 | 2226 | aZ[1] = aZfilter10Hz; |
jjmedina | 0:a5367bd4e404 | 2227 | azfilter1Hz[1] = aZfilter10Hz; |
jjmedina | 0:a5367bd4e404 | 2228 | |
jjmedina | 0:a5367bd4e404 | 2229 | fprintf(fp2,"%f %f %f %f %f %f \n", t, dZ, vZ, aZfilter10Hz, jerk, azfilter1Hz[1]); |
jjmedina | 0:a5367bd4e404 | 2230 | |
jjmedina | 0:a5367bd4e404 | 2231 | }else |
jjmedina | 0:a5367bd4e404 | 2232 | { |
jjmedina | 0:a5367bd4e404 | 2233 | //azfilter1Hz |
jjmedina | 0:a5367bd4e404 | 2234 | aZ[2] = aZfilter10Hz; |
jjmedina | 0:a5367bd4e404 | 2235 | if ( FrecAdq == 320 ) |
jjmedina | 0:a5367bd4e404 | 2236 | { |
jjmedina | 0:a5367bd4e404 | 2237 | azfilter1Hz[2] = toLowPass1HzO2_320(azfilter1Hz[0],azfilter1Hz[1],aZ); |
jjmedina | 0:a5367bd4e404 | 2238 | |
jjmedina | 0:a5367bd4e404 | 2239 | }else if ( FrecAdq == 1000 ) |
jjmedina | 0:a5367bd4e404 | 2240 | { |
jjmedina | 0:a5367bd4e404 | 2241 | azfilter1Hz[2] = toLowPass1HzO2_1000(azfilter1Hz[0],azfilter1Hz[1],aZ); |
jjmedina | 0:a5367bd4e404 | 2242 | }else//seguridad |
jjmedina | 0:a5367bd4e404 | 2243 | { |
jjmedina | 0:a5367bd4e404 | 2244 | azfilter1Hz[2] = toLowPass1HzO2_320(azfilter1Hz[0],azfilter1Hz[1],aZ); |
jjmedina | 0:a5367bd4e404 | 2245 | } |
jjmedina | 0:a5367bd4e404 | 2246 | |
jjmedina | 0:a5367bd4e404 | 2247 | fprintf(fp2,"%f %f %f %f %f %f \n", t, dZ, vZ, aZfilter10Hz, jerk, azfilter1Hz[2]); |
jjmedina | 0:a5367bd4e404 | 2248 | //updates |
jjmedina | 0:a5367bd4e404 | 2249 | aZ[0] = aZ[1]; |
jjmedina | 0:a5367bd4e404 | 2250 | aZ[1] = aZ[2]; |
jjmedina | 0:a5367bd4e404 | 2251 | //azfilter1Hz |
jjmedina | 0:a5367bd4e404 | 2252 | azfilter1Hz[0] = azfilter1Hz[1]; |
jjmedina | 0:a5367bd4e404 | 2253 | azfilter1Hz[1] = azfilter1Hz[2]; |
jjmedina | 0:a5367bd4e404 | 2254 | } |
jjmedina | 0:a5367bd4e404 | 2255 | |
jjmedina | 0:a5367bd4e404 | 2256 | }//fin pasada |
jjmedina | 0:a5367bd4e404 | 2257 | fclose(fp); |
jjmedina | 0:a5367bd4e404 | 2258 | fclose(fp2); |
jjmedina | 0:a5367bd4e404 | 2259 | //int rmDVA0 = remove(dispVelFile);//elimina dvaj_ |
jjmedina | 0:a5367bd4e404 | 2260 | |
jjmedina | 0:a5367bd4e404 | 2261 | //segunda pasada. Calcula step2 |
jjmedina | 0:a5367bd4e404 | 2262 | strcpy(dispVelFile,"/local/dvaja_"); |
jjmedina | 0:a5367bd4e404 | 2263 | strcpy(velMod,"/local/dvaj2_"); |
jjmedina | 0:a5367bd4e404 | 2264 | |
jjmedina | 0:a5367bd4e404 | 2265 | sprintf (NoViajel, "%d", ContadorEnsayos); |
jjmedina | 0:a5367bd4e404 | 2266 | strcat(dispVelFile,NoViajel); strcat(velMod,NoViajel); |
jjmedina | 0:a5367bd4e404 | 2267 | strcat(dispVelFile,".txt"); strcat(velMod,".txt"); |
jjmedina | 0:a5367bd4e404 | 2268 | |
jjmedina | 0:a5367bd4e404 | 2269 | fp = fopen(dispVelFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", dispVelFile); } |
jjmedina | 0:a5367bd4e404 | 2270 | fp2 = fopen(velMod,"w"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", velMod); } |
jjmedina | 0:a5367bd4e404 | 2271 | |
jjmedina | 0:a5367bd4e404 | 2272 | if (nDatos < 0) {nDatos = 0;} |
jjmedina | 0:a5367bd4e404 | 2273 | |
jjmedina | 0:a5367bd4e404 | 2274 | for(unsigned int i = 0; i<nDatos; i++) |
jjmedina | 0:a5367bd4e404 | 2275 | { |
jjmedina | 0:a5367bd4e404 | 2276 | //fscanf(fp,"%f %f %f %f %f %f", &t, &dZ, &vZ, &aZ, &aX, &aY);//aZ filtered, aX y aY raw. |
jjmedina | 0:a5367bd4e404 | 2277 | fscanf(fp,"%f %f %f %f %f %f", &t, &dZ, &vZ, &aZfilter10Hz, &jerk, &azfilter1Hz[2]); |
jjmedina | 0:a5367bd4e404 | 2278 | //Calculo de step2 por regresion |
jjmedina | 0:a5367bd4e404 | 2279 | |
jjmedina | 0:a5367bd4e404 | 2280 | for(unsigned int j = 0; j<(2*Ai+1); j++) { azfilter1Hz_1s[j] = azfilter1Hz_1s[j+1]; } |
jjmedina | 0:a5367bd4e404 | 2281 | azfilter1Hz_1s[2*Ai] = azfilter1Hz[2]; |
jjmedina | 0:a5367bd4e404 | 2282 | for(unsigned int j = 0; j<(Ai+1); j++) { step2_05s[j] = step2_05s[j+1]; } |
jjmedina | 0:a5367bd4e404 | 2283 | step2_05s[Ai] = RegresionJerk(azfilter1Hz_1s, 2*Ai+1); |
jjmedina | 0:a5367bd4e404 | 2284 | |
jjmedina | 0:a5367bd4e404 | 2285 | fprintf(fp2,"%f %f %f %f %f %f \n", t, dZ, vZ, aZfilter10Hz, jerk, step2_05s[0]); |
jjmedina | 0:a5367bd4e404 | 2286 | } |
jjmedina | 0:a5367bd4e404 | 2287 | |
jjmedina | 0:a5367bd4e404 | 2288 | fclose(fp); |
jjmedina | 0:a5367bd4e404 | 2289 | fclose(fp2); |
jjmedina | 0:a5367bd4e404 | 2290 | int rmDVA1 = remove(dispVelFile); |
jjmedina | 0:a5367bd4e404 | 2291 | |
jjmedina | 0:a5367bd4e404 | 2292 | } |
jjmedina | 0:a5367bd4e404 | 2293 | |
jjmedina | 0:a5367bd4e404 | 2294 | void calculaCarNCar(unsigned int ContadorEnsayos, unsigned int ContadorDatos, int PasoTiempo) |
jjmedina | 0:a5367bd4e404 | 2295 | { |
jjmedina | 0:a5367bd4e404 | 2296 | float t; |
jjmedina | 0:a5367bd4e404 | 2297 | float dZ,vZ,aZfilter10Hz,jerk,step2; |
jjmedina | 0:a5367bd4e404 | 2298 | |
jjmedina | 0:a5367bd4e404 | 2299 | const float timeStep = (float)(PasoTiempo/1000000.0); |
jjmedina | 0:a5367bd4e404 | 2300 | const float Atadq = timeStep; |
jjmedina | 0:a5367bd4e404 | 2301 | |
jjmedina | 0:a5367bd4e404 | 2302 | //Archivos de lectura y escritura |
jjmedina | 0:a5367bd4e404 | 2303 | FILE * fp; |
jjmedina | 0:a5367bd4e404 | 2304 | |
jjmedina | 0:a5367bd4e404 | 2305 | char NoViajel[2]; |
jjmedina | 0:a5367bd4e404 | 2306 | char dispVelFile[50]; |
jjmedina | 0:a5367bd4e404 | 2307 | |
jjmedina | 0:a5367bd4e404 | 2308 | strcpy(dispVelFile,"/local/dvaj2_"); |
jjmedina | 0:a5367bd4e404 | 2309 | sprintf (NoViajel, "%d", ContadorEnsayos); |
jjmedina | 0:a5367bd4e404 | 2310 | strcat(dispVelFile,NoViajel); |
jjmedina | 0:a5367bd4e404 | 2311 | strcat(dispVelFile,".txt"); |
jjmedina | 0:a5367bd4e404 | 2312 | |
jjmedina | 0:a5367bd4e404 | 2313 | fp = fopen(dispVelFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", dispVelFile); } |
jjmedina | 0:a5367bd4e404 | 2314 | |
jjmedina | 0:a5367bd4e404 | 2315 | int control(0); |
jjmedina | 0:a5367bd4e404 | 2316 | vector <int> regiones; |
jjmedina | 0:a5367bd4e404 | 2317 | |
jjmedina | 0:a5367bd4e404 | 2318 | int nDatos = (ContadorDatos-(ndes+nmed)); |
jjmedina | 0:a5367bd4e404 | 2319 | if (nDatos < 0) {nDatos = 0;} |
jjmedina | 0:a5367bd4e404 | 2320 | |
jjmedina | 0:a5367bd4e404 | 2321 | for(int i = 0; i<nDatos; i++) |
jjmedina | 0:a5367bd4e404 | 2322 | { |
jjmedina | 0:a5367bd4e404 | 2323 | |
jjmedina | 0:a5367bd4e404 | 2324 | fscanf(fp,"%f %f %f %f %f %f \n", &t, &dZ, &vZ, &aZfilter10Hz, &jerk, &step2);//aZ filtered, aX y aY raw. |
jjmedina | 0:a5367bd4e404 | 2325 | |
jjmedina | 0:a5367bd4e404 | 2326 | //Step 3: Identifica slopes con valor mayor que 0.3 m/s^3 |
jjmedina | 0:a5367bd4e404 | 2327 | if ( (i >= Bo0) && (i <= Bo3) ) |
jjmedina | 0:a5367bd4e404 | 2328 | { |
jjmedina | 0:a5367bd4e404 | 2329 | if ((fabs(step2)>=0.3)&&(control==0)) |
jjmedina | 0:a5367bd4e404 | 2330 | { |
jjmedina | 0:a5367bd4e404 | 2331 | |
jjmedina | 0:a5367bd4e404 | 2332 | regiones.push_back(i-((int)(0.5/timeStep)));//correccion indice por retraso step2 en 0.5 segundos OJO |
jjmedina | 0:a5367bd4e404 | 2333 | // regiones.push_back(i);//correccion indice por retraso step2 en 0.5 segundos OJO |
jjmedina | 0:a5367bd4e404 | 2334 | control=1; |
jjmedina | 0:a5367bd4e404 | 2335 | } |
jjmedina | 0:a5367bd4e404 | 2336 | else if ((fabs(step2)<=0.3)&&(control==1)) |
jjmedina | 0:a5367bd4e404 | 2337 | { |
jjmedina | 0:a5367bd4e404 | 2338 | control=0; |
jjmedina | 0:a5367bd4e404 | 2339 | regiones.push_back(i-1-((int)(0.5/timeStep)));//correccion indice por retraso step2 en 0.5 segundos |
jjmedina | 0:a5367bd4e404 | 2340 | // regiones.push_back(i-1);//correccion indice por retraso step2 en 0.5 segundos |
jjmedina | 0:a5367bd4e404 | 2341 | } |
jjmedina | 0:a5367bd4e404 | 2342 | } |
jjmedina | 0:a5367bd4e404 | 2343 | }//fin loop AQUI---abajo, cambiar time.at(regiones.at(2*i)) por tiempo sabiendo el paso etc. |
jjmedina | 0:a5367bd4e404 | 2344 | |
jjmedina | 0:a5367bd4e404 | 2345 | //Step 4: Add 0.5 s . Hecho en el bucle de abajo. al primero le resto 0.5 y |
jjmedina | 0:a5367bd4e404 | 2346 | // al segundo se lo sumo. Para los indices como conozco Atadq, sumo y resto |
jjmedina | 0:a5367bd4e404 | 2347 | // 0.5/Atadq |
jjmedina | 0:a5367bd4e404 | 2348 | Nregiones = (int)(regiones.size()/2.0); |
jjmedina | 0:a5367bd4e404 | 2349 | //region [t1 t2 index(t1) index(t1)] |
jjmedina | 0:a5367bd4e404 | 2350 | |
jjmedina | 0:a5367bd4e404 | 2351 | if(Nregiones > 0) |
jjmedina | 0:a5367bd4e404 | 2352 | { |
jjmedina | 0:a5367bd4e404 | 2353 | region = new float* [Nregiones]; |
jjmedina | 0:a5367bd4e404 | 2354 | for(unsigned int i = 0; i < Nregiones; i++) |
jjmedina | 0:a5367bd4e404 | 2355 | region[i] = new float[4]; |
jjmedina | 0:a5367bd4e404 | 2356 | |
jjmedina | 0:a5367bd4e404 | 2357 | |
jjmedina | 0:a5367bd4e404 | 2358 | for (unsigned int i = 0; i < Nregiones; i++) |
jjmedina | 0:a5367bd4e404 | 2359 | { |
jjmedina | 0:a5367bd4e404 | 2360 | float factorT(0.5); |
jjmedina | 0:a5367bd4e404 | 2361 | // while ( ((timeStep*regiones.at(2*i)-factorT)<0) || ((timeStep*regiones.at(2*i +1)+factorT)>time.at(time.size()-1)) ) |
jjmedina | 0:a5367bd4e404 | 2362 | while ( ((timeStep*regiones.at(2*i)-factorT)<0) || ((timeStep*regiones.at(2*i +1)+factorT)>timeStep*Bo3) ) |
jjmedina | 0:a5367bd4e404 | 2363 | { |
jjmedina | 0:a5367bd4e404 | 2364 | factorT = factorT - 0.1; |
jjmedina | 0:a5367bd4e404 | 2365 | } |
jjmedina | 0:a5367bd4e404 | 2366 | region[i][0] = timeStep*regiones.at(2*i)-factorT; |
jjmedina | 0:a5367bd4e404 | 2367 | region[i][2] = regiones.at(2*i)-factorT/Atadq; |
jjmedina | 0:a5367bd4e404 | 2368 | |
jjmedina | 0:a5367bd4e404 | 2369 | region[i][1] = timeStep*regiones.at(2*i +1)+factorT; |
jjmedina | 0:a5367bd4e404 | 2370 | region[i][3] = regiones.at(2*i + 1)+factorT/Atadq; |
jjmedina | 0:a5367bd4e404 | 2371 | } |
jjmedina | 0:a5367bd4e404 | 2372 | // Step 5: cada fila de region (=Nregiones) es una region de non-constant |
jjmedina | 0:a5367bd4e404 | 2373 | // acceleration |
jjmedina | 0:a5367bd4e404 | 2374 | // Step 6: define regiones de constant acceleration (car) |
jjmedina | 0:a5367bd4e404 | 2375 | |
jjmedina | 0:a5367bd4e404 | 2376 | int cont = 0; |
jjmedina | 0:a5367bd4e404 | 2377 | |
jjmedina | 0:a5367bd4e404 | 2378 | for (unsigned int i=0; i < (Nregiones-1); i++) |
jjmedina | 0:a5367bd4e404 | 2379 | { |
jjmedina | 0:a5367bd4e404 | 2380 | if ( region[i+1][2] > region[i][3] ) |
jjmedina | 0:a5367bd4e404 | 2381 | { |
jjmedina | 0:a5367bd4e404 | 2382 | |
jjmedina | 0:a5367bd4e404 | 2383 | regionconst[cont][0]=region[i][1]; |
jjmedina | 0:a5367bd4e404 | 2384 | regionconst[cont][1]=region[i+1][0]; |
jjmedina | 0:a5367bd4e404 | 2385 | regionconst[cont][2]=region[i][3]; |
jjmedina | 0:a5367bd4e404 | 2386 | regionconst[cont][3]=region[i+1][2]; |
jjmedina | 0:a5367bd4e404 | 2387 | cont++; |
jjmedina | 0:a5367bd4e404 | 2388 | } |
jjmedina | 0:a5367bd4e404 | 2389 | } |
jjmedina | 0:a5367bd4e404 | 2390 | Nregionesconst = cont; |
jjmedina | 0:a5367bd4e404 | 2391 | } |
jjmedina | 0:a5367bd4e404 | 2392 | else |
jjmedina | 0:a5367bd4e404 | 2393 | { |
jjmedina | 0:a5367bd4e404 | 2394 | Nregionesconst = 0; |
jjmedina | 0:a5367bd4e404 | 2395 | } |
jjmedina | 0:a5367bd4e404 | 2396 | |
jjmedina | 0:a5367bd4e404 | 2397 | fclose(fp); |
jjmedina | 0:a5367bd4e404 | 2398 | int rmDVA0 = remove(dispVelFile); |
jjmedina | 0:a5367bd4e404 | 2399 | |
jjmedina | 0:a5367bd4e404 | 2400 | } |
jjmedina | 0:a5367bd4e404 | 2401 | |
jjmedina | 0:a5367bd4e404 | 2402 | void calculaZXYvibweighted( unsigned int ContadorEnsayos, unsigned int ContadorDatos) |
jjmedina | 0:a5367bd4e404 | 2403 | { |
jjmedina | 0:a5367bd4e404 | 2404 | //aux data |
jjmedina | 0:a5367bd4e404 | 2405 | float Zvibweighted_max(0.0); |
jjmedina | 0:a5367bd4e404 | 2406 | float Zvibweighted_min(0.0); |
jjmedina | 0:a5367bd4e404 | 2407 | float Zvibweighted_max_ncar(0.0); |
jjmedina | 0:a5367bd4e404 | 2408 | float Zvibweighted_min_ncar(0.0); |
jjmedina | 0:a5367bd4e404 | 2409 | float Xvibweighted_max(0.0); |
jjmedina | 0:a5367bd4e404 | 2410 | float Xvibweighted_min(0.0); |
jjmedina | 0:a5367bd4e404 | 2411 | float Yvibweighted_max(0.0); |
jjmedina | 0:a5367bd4e404 | 2412 | float Yvibweighted_min(0.0); |
jjmedina | 0:a5367bd4e404 | 2413 | int signoCompara; |
jjmedina | 0:a5367bd4e404 | 2414 | int signoCompara_ncar; |
jjmedina | 0:a5367bd4e404 | 2415 | int signoCompara_X; |
jjmedina | 0:a5367bd4e404 | 2416 | int signoCompara_Y; |
jjmedina | 0:a5367bd4e404 | 2417 | int signoControl; |
jjmedina | 0:a5367bd4e404 | 2418 | int signoControl_X; |
jjmedina | 0:a5367bd4e404 | 2419 | int signoControl_Y; |
jjmedina | 0:a5367bd4e404 | 2420 | int contadorCeros(0); |
jjmedina | 0:a5367bd4e404 | 2421 | int contadorCeros_ncar(0); |
jjmedina | 0:a5367bd4e404 | 2422 | int contadorCeros_X(0); |
jjmedina | 0:a5367bd4e404 | 2423 | int contadorCeros_Y(0); |
jjmedina | 0:a5367bd4e404 | 2424 | |
jjmedina | 0:a5367bd4e404 | 2425 | |
jjmedina | 0:a5367bd4e404 | 2426 | float difZ(0.0); |
jjmedina | 0:a5367bd4e404 | 2427 | float difX(0.0); |
jjmedina | 0:a5367bd4e404 | 2428 | float difY(0.0); |
jjmedina | 0:a5367bd4e404 | 2429 | int contador_peaktopeak_car(0); |
jjmedina | 0:a5367bd4e404 | 2430 | int contador_peaktopeak_X(0); |
jjmedina | 0:a5367bd4e404 | 2431 | int contador_peaktopeak_Y(0); |
jjmedina | 0:a5367bd4e404 | 2432 | |
jjmedina | 0:a5367bd4e404 | 2433 | int marca(-1);//-1 = no |
jjmedina | 0:a5367bd4e404 | 2434 | |
jjmedina | 0:a5367bd4e404 | 2435 | //read data from file |
jjmedina | 0:a5367bd4e404 | 2436 | float Xvibweighted; |
jjmedina | 0:a5367bd4e404 | 2437 | float Yvibweighted; |
jjmedina | 0:a5367bd4e404 | 2438 | float Zvibweighted; |
jjmedina | 0:a5367bd4e404 | 2439 | float t; |
jjmedina | 0:a5367bd4e404 | 2440 | |
jjmedina | 0:a5367bd4e404 | 2441 | |
jjmedina | 0:a5367bd4e404 | 2442 | //Archivos de lectura y escritura |
jjmedina | 0:a5367bd4e404 | 2443 | FILE * fp; |
jjmedina | 0:a5367bd4e404 | 2444 | FILE * fp2; |
jjmedina | 0:a5367bd4e404 | 2445 | FILE * fp3; |
jjmedina | 0:a5367bd4e404 | 2446 | FILE * fp4; |
jjmedina | 0:a5367bd4e404 | 2447 | |
jjmedina | 0:a5367bd4e404 | 2448 | char NoViajel[2]; |
jjmedina | 0:a5367bd4e404 | 2449 | char dispVelFile[50]; |
jjmedina | 0:a5367bd4e404 | 2450 | char A95file[50]; |
jjmedina | 0:a5367bd4e404 | 2451 | char A95fileX[50]; |
jjmedina | 0:a5367bd4e404 | 2452 | char A95fileY[50]; |
jjmedina | 0:a5367bd4e404 | 2453 | |
jjmedina | 0:a5367bd4e404 | 2454 | strcpy(dispVelFile,"/local/W5_IV_"); |
jjmedina | 0:a5367bd4e404 | 2455 | strcpy(A95file,"/local/A95_IV"); |
jjmedina | 0:a5367bd4e404 | 2456 | strcpy(A95fileX,"/local/AX_IV_"); |
jjmedina | 0:a5367bd4e404 | 2457 | strcpy(A95fileY,"/local/AY_IV_"); |
jjmedina | 0:a5367bd4e404 | 2458 | sprintf (NoViajel, "%d", ContadorEnsayos); |
jjmedina | 0:a5367bd4e404 | 2459 | strcat(dispVelFile,NoViajel); |
jjmedina | 0:a5367bd4e404 | 2460 | strcat(A95file,NoViajel); |
jjmedina | 0:a5367bd4e404 | 2461 | strcat(A95fileX,NoViajel); |
jjmedina | 0:a5367bd4e404 | 2462 | strcat(A95fileY,NoViajel); |
jjmedina | 0:a5367bd4e404 | 2463 | strcat(dispVelFile,".txt"); |
jjmedina | 0:a5367bd4e404 | 2464 | strcat(A95file,".txt"); |
jjmedina | 0:a5367bd4e404 | 2465 | strcat(A95fileX,".txt"); |
jjmedina | 0:a5367bd4e404 | 2466 | strcat(A95fileY,".txt"); |
jjmedina | 0:a5367bd4e404 | 2467 | |
jjmedina | 0:a5367bd4e404 | 2468 | |
jjmedina | 0:a5367bd4e404 | 2469 | fp = fopen(dispVelFile,"r"); if(fp == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for read\n", dispVelFile); } |
jjmedina | 0:a5367bd4e404 | 2470 | fp2 = fopen(A95file,"w"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95file); } |
jjmedina | 0:a5367bd4e404 | 2471 | fp3 = fopen(A95fileX,"w"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileX); } |
jjmedina | 0:a5367bd4e404 | 2472 | fp4 = fopen(A95fileY,"w"); if(fp4 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileY); } |
jjmedina | 0:a5367bd4e404 | 2473 | |
jjmedina | 0:a5367bd4e404 | 2474 | int nDatos = (ContadorDatos-(ndes+nmed)); |
jjmedina | 0:a5367bd4e404 | 2475 | if (nDatos < 0) {nDatos = 0;} |
jjmedina | 0:a5367bd4e404 | 2476 | |
jjmedina | 0:a5367bd4e404 | 2477 | for(unsigned int i = 0; i<nDatos; i++)//-------------------------------------------------------------MAIN LOOP |
jjmedina | 0:a5367bd4e404 | 2478 | { |
jjmedina | 0:a5367bd4e404 | 2479 | fscanf(fp,"%f %f %f %f", &t, &Xvibweighted, &Yvibweighted, &Zvibweighted);//lee tiempo y 3 aceleraciones ponderadas X, Y, Z |
jjmedina | 0:a5367bd4e404 | 2480 | |
jjmedina | 0:a5367bd4e404 | 2481 | /////////////////////////////////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 2482 | ////////////////Zvib/////////////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 2483 | /////////////////////////////////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 2484 | //signo |
jjmedina | 0:a5367bd4e404 | 2485 | signoControl = (int)(Zvibweighted/fabs(Zvibweighted));//+1 positivo, -1 negativo |
jjmedina | 0:a5367bd4e404 | 2486 | |
jjmedina | 0:a5367bd4e404 | 2487 | ////////////////////////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 2488 | //Constant acceleration Region (subindex=car). Maximum peak to peak y A95 peak to peak |
jjmedina | 0:a5367bd4e404 | 2489 | //comprueba que esta dentro de alguna de las fronteras |
jjmedina | 0:a5367bd4e404 | 2490 | if ( Nregionesconst == 0 ) |
jjmedina | 0:a5367bd4e404 | 2491 | { |
jjmedina | 0:a5367bd4e404 | 2492 | max_peaktopeak_car_ = 0.0; |
jjmedina | 0:a5367bd4e404 | 2493 | A95_peaktopeak_car_ = 0.0; |
jjmedina | 0:a5367bd4e404 | 2494 | }else |
jjmedina | 0:a5367bd4e404 | 2495 | { |
jjmedina | 0:a5367bd4e404 | 2496 | for (int j = 0 ; j < Nregionesconst ; j++) |
jjmedina | 0:a5367bd4e404 | 2497 | { |
jjmedina | 0:a5367bd4e404 | 2498 | if ( i == ((int)regionconst[j][2]) ) |
jjmedina | 0:a5367bd4e404 | 2499 | { |
jjmedina | 0:a5367bd4e404 | 2500 | signoCompara = (int)(Zvibweighted/fabs(Zvibweighted));//+1 positivo, -1 negativo |
jjmedina | 0:a5367bd4e404 | 2501 | } |
jjmedina | 0:a5367bd4e404 | 2502 | if ( (((int)regionconst[j][2]) < i) && (i < (int)regionconst[j][3]) ) |
jjmedina | 0:a5367bd4e404 | 2503 | { |
jjmedina | 0:a5367bd4e404 | 2504 | if ( (signoControl-signoCompara) != 0 ) |
jjmedina | 0:a5367bd4e404 | 2505 | { |
jjmedina | 0:a5367bd4e404 | 2506 | contadorCeros++; |
jjmedina | 0:a5367bd4e404 | 2507 | signoCompara = signoControl; |
jjmedina | 0:a5367bd4e404 | 2508 | } |
jjmedina | 0:a5367bd4e404 | 2509 | if ( (1 <= contadorCeros) && (contadorCeros < 3) ) |
jjmedina | 0:a5367bd4e404 | 2510 | { |
jjmedina | 0:a5367bd4e404 | 2511 | if ( Zvibweighted > Zvibweighted_max ) |
jjmedina | 0:a5367bd4e404 | 2512 | { |
jjmedina | 0:a5367bd4e404 | 2513 | Zvibweighted_max = Zvibweighted; |
jjmedina | 0:a5367bd4e404 | 2514 | } |
jjmedina | 0:a5367bd4e404 | 2515 | if ( Zvibweighted < Zvibweighted_min ) |
jjmedina | 0:a5367bd4e404 | 2516 | { |
jjmedina | 0:a5367bd4e404 | 2517 | Zvibweighted_min = Zvibweighted; |
jjmedina | 0:a5367bd4e404 | 2518 | } |
jjmedina | 0:a5367bd4e404 | 2519 | }else if (contadorCeros == 3) |
jjmedina | 0:a5367bd4e404 | 2520 | { |
jjmedina | 0:a5367bd4e404 | 2521 | contadorCeros = 0; |
jjmedina | 0:a5367bd4e404 | 2522 | difZ = (Zvibweighted_max - Zvibweighted_min); |
jjmedina | 0:a5367bd4e404 | 2523 | if ( difZ > max_peaktopeak_car_ ){max_peaktopeak_car_ = difZ;} |
jjmedina | 0:a5367bd4e404 | 2524 | //almacena para A95 |
jjmedina | 0:a5367bd4e404 | 2525 | fprintf(fp2,"%f \n", difZ); |
jjmedina | 0:a5367bd4e404 | 2526 | |
jjmedina | 0:a5367bd4e404 | 2527 | contador_peaktopeak_car++; |
jjmedina | 0:a5367bd4e404 | 2528 | Zvibweighted_max = 0.0; |
jjmedina | 0:a5367bd4e404 | 2529 | Zvibweighted_min = 0.0; |
jjmedina | 0:a5367bd4e404 | 2530 | } |
jjmedina | 0:a5367bd4e404 | 2531 | } |
jjmedina | 0:a5367bd4e404 | 2532 | }//for Nregionesconst |
jjmedina | 0:a5367bd4e404 | 2533 | }//else |
jjmedina | 0:a5367bd4e404 | 2534 | |
jjmedina | 0:a5367bd4e404 | 2535 | |
jjmedina | 0:a5367bd4e404 | 2536 | ////////////////////////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 2537 | //Non-constant acceleration Region (subindex=ncar). Maximum peak to peak |
jjmedina | 0:a5367bd4e404 | 2538 | //comprueba que esta dentro de alguna de las fronteras |
jjmedina | 0:a5367bd4e404 | 2539 | if ( Nregiones == 0 ) |
jjmedina | 0:a5367bd4e404 | 2540 | { |
jjmedina | 0:a5367bd4e404 | 2541 | max_peaktopeak_ncar_ = 0.0; |
jjmedina | 0:a5367bd4e404 | 2542 | }else |
jjmedina | 0:a5367bd4e404 | 2543 | { |
jjmedina | 0:a5367bd4e404 | 2544 | for (int j = 0 ; j < Nregiones ; j++) |
jjmedina | 0:a5367bd4e404 | 2545 | { |
jjmedina | 0:a5367bd4e404 | 2546 | if ( i == ((int)region[j][2]) ) |
jjmedina | 0:a5367bd4e404 | 2547 | { |
jjmedina | 0:a5367bd4e404 | 2548 | signoCompara_ncar = (int)(Zvibweighted/fabs(Zvibweighted));//+1 positivo, -1 negativo |
jjmedina | 0:a5367bd4e404 | 2549 | marca = j; |
jjmedina | 0:a5367bd4e404 | 2550 | } |
jjmedina | 0:a5367bd4e404 | 2551 | if ( (((int)region[j][2]) < i) && (i < (int)region[j][3]) ) |
jjmedina | 0:a5367bd4e404 | 2552 | { |
jjmedina | 0:a5367bd4e404 | 2553 | if ( (signoControl-signoCompara_ncar) != 0 ) |
jjmedina | 0:a5367bd4e404 | 2554 | { |
jjmedina | 0:a5367bd4e404 | 2555 | contadorCeros_ncar++; |
jjmedina | 0:a5367bd4e404 | 2556 | signoCompara_ncar = signoControl; |
jjmedina | 0:a5367bd4e404 | 2557 | } |
jjmedina | 0:a5367bd4e404 | 2558 | if ( (1 <= contadorCeros_ncar) && (contadorCeros_ncar < 3))// && (marca == j) ) |
jjmedina | 0:a5367bd4e404 | 2559 | { |
jjmedina | 0:a5367bd4e404 | 2560 | if ( (Zvibweighted > Zvibweighted_max_ncar) && (marca == j) ) |
jjmedina | 0:a5367bd4e404 | 2561 | { |
jjmedina | 0:a5367bd4e404 | 2562 | Zvibweighted_max_ncar = Zvibweighted; |
jjmedina | 0:a5367bd4e404 | 2563 | } |
jjmedina | 0:a5367bd4e404 | 2564 | if ( (Zvibweighted < Zvibweighted_min_ncar) && (marca == j) ) |
jjmedina | 0:a5367bd4e404 | 2565 | { |
jjmedina | 0:a5367bd4e404 | 2566 | Zvibweighted_min_ncar = Zvibweighted; |
jjmedina | 0:a5367bd4e404 | 2567 | } |
jjmedina | 0:a5367bd4e404 | 2568 | }else |
jjmedina | 0:a5367bd4e404 | 2569 | { |
jjmedina | 0:a5367bd4e404 | 2570 | contadorCeros_ncar = 0; //marca = -1; |
jjmedina | 0:a5367bd4e404 | 2571 | if ( (Zvibweighted_max_ncar - Zvibweighted_min_ncar) > max_peaktopeak_ncar_ ){max_peaktopeak_ncar_ = (Zvibweighted_max_ncar - Zvibweighted_min_ncar);} |
jjmedina | 0:a5367bd4e404 | 2572 | Zvibweighted_max_ncar = 0.0; |
jjmedina | 0:a5367bd4e404 | 2573 | Zvibweighted_min_ncar = 0.0; |
jjmedina | 0:a5367bd4e404 | 2574 | } |
jjmedina | 0:a5367bd4e404 | 2575 | } |
jjmedina | 0:a5367bd4e404 | 2576 | }//for Nregiones |
jjmedina | 0:a5367bd4e404 | 2577 | }//else |
jjmedina | 0:a5367bd4e404 | 2578 | |
jjmedina | 0:a5367bd4e404 | 2579 | ////////////////////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 2580 | ////////Xvib///////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 2581 | ////////////////////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 2582 | //signo |
jjmedina | 0:a5367bd4e404 | 2583 | signoControl_X = (int)(Xvibweighted/fabs(Xvibweighted));//+1 positivo, -1 negativo |
jjmedina | 0:a5367bd4e404 | 2584 | if ( i == Bo1 ) |
jjmedina | 0:a5367bd4e404 | 2585 | { |
jjmedina | 0:a5367bd4e404 | 2586 | signoCompara_X = (int)(Xvibweighted/fabs(Xvibweighted));//+1 positivo, -1 negativo |
jjmedina | 0:a5367bd4e404 | 2587 | } |
jjmedina | 0:a5367bd4e404 | 2588 | if ( Bo1 == Bo2 ) |
jjmedina | 0:a5367bd4e404 | 2589 | { |
jjmedina | 0:a5367bd4e404 | 2590 | max_peaktopeak_X_ = 0.0; |
jjmedina | 0:a5367bd4e404 | 2591 | }else |
jjmedina | 0:a5367bd4e404 | 2592 | { |
jjmedina | 0:a5367bd4e404 | 2593 | if (( Bo1 < i) && (i < Bo2 )) |
jjmedina | 0:a5367bd4e404 | 2594 | { |
jjmedina | 0:a5367bd4e404 | 2595 | if ( (signoControl_X-signoCompara_X) != 0 ) |
jjmedina | 0:a5367bd4e404 | 2596 | { |
jjmedina | 0:a5367bd4e404 | 2597 | contadorCeros_X++; |
jjmedina | 0:a5367bd4e404 | 2598 | signoCompara_X = signoControl_X; |
jjmedina | 0:a5367bd4e404 | 2599 | } |
jjmedina | 0:a5367bd4e404 | 2600 | if ( (1 <= contadorCeros_X) && (contadorCeros_X < 3) ) |
jjmedina | 0:a5367bd4e404 | 2601 | { |
jjmedina | 0:a5367bd4e404 | 2602 | if ( Xvibweighted > Xvibweighted_max ) |
jjmedina | 0:a5367bd4e404 | 2603 | { |
jjmedina | 0:a5367bd4e404 | 2604 | Xvibweighted_max = Xvibweighted; |
jjmedina | 0:a5367bd4e404 | 2605 | } |
jjmedina | 0:a5367bd4e404 | 2606 | if ( Xvibweighted < Xvibweighted_min ) |
jjmedina | 0:a5367bd4e404 | 2607 | { |
jjmedina | 0:a5367bd4e404 | 2608 | Xvibweighted_min = Xvibweighted; |
jjmedina | 0:a5367bd4e404 | 2609 | } |
jjmedina | 0:a5367bd4e404 | 2610 | }else if (contadorCeros_X == 3) |
jjmedina | 0:a5367bd4e404 | 2611 | { |
jjmedina | 0:a5367bd4e404 | 2612 | contadorCeros_X = 0; |
jjmedina | 0:a5367bd4e404 | 2613 | difX = (Xvibweighted_max - Xvibweighted_min); |
jjmedina | 0:a5367bd4e404 | 2614 | if ( difX > max_peaktopeak_X_ ){max_peaktopeak_X_ = difX;} |
jjmedina | 0:a5367bd4e404 | 2615 | //almacena para A95 |
jjmedina | 0:a5367bd4e404 | 2616 | fprintf(fp3,"%f \n", difX); |
jjmedina | 0:a5367bd4e404 | 2617 | contador_peaktopeak_X++; |
jjmedina | 0:a5367bd4e404 | 2618 | Xvibweighted_max = 0.0; |
jjmedina | 0:a5367bd4e404 | 2619 | Xvibweighted_min = 0.0; |
jjmedina | 0:a5367bd4e404 | 2620 | } |
jjmedina | 0:a5367bd4e404 | 2621 | } |
jjmedina | 0:a5367bd4e404 | 2622 | } |
jjmedina | 0:a5367bd4e404 | 2623 | |
jjmedina | 0:a5367bd4e404 | 2624 | ////////////////////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 2625 | ////////Yvib///////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 2626 | ////////////////////////////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 2627 | //signo |
jjmedina | 0:a5367bd4e404 | 2628 | signoControl_Y = (int)(Yvibweighted/fabs(Yvibweighted));//+1 positivo, -1 negativo |
jjmedina | 0:a5367bd4e404 | 2629 | if ( i == Bo1 ) |
jjmedina | 0:a5367bd4e404 | 2630 | { |
jjmedina | 0:a5367bd4e404 | 2631 | signoCompara_Y = (int)(Yvibweighted/fabs(Yvibweighted));//+1 positivo, -1 negativo |
jjmedina | 0:a5367bd4e404 | 2632 | } |
jjmedina | 0:a5367bd4e404 | 2633 | if ( Bo1 == Bo2 ) |
jjmedina | 0:a5367bd4e404 | 2634 | { |
jjmedina | 0:a5367bd4e404 | 2635 | max_peaktopeak_Y_ = 0.0; |
jjmedina | 0:a5367bd4e404 | 2636 | }else |
jjmedina | 0:a5367bd4e404 | 2637 | { |
jjmedina | 0:a5367bd4e404 | 2638 | if (( Bo1 < i) && (i < Bo2 )) |
jjmedina | 0:a5367bd4e404 | 2639 | { |
jjmedina | 0:a5367bd4e404 | 2640 | if ( (signoControl_Y-signoCompara_Y) != 0 ) |
jjmedina | 0:a5367bd4e404 | 2641 | { |
jjmedina | 0:a5367bd4e404 | 2642 | contadorCeros_Y++; |
jjmedina | 0:a5367bd4e404 | 2643 | signoCompara_Y = signoControl_Y; |
jjmedina | 0:a5367bd4e404 | 2644 | } |
jjmedina | 0:a5367bd4e404 | 2645 | if ( (1 <= contadorCeros_Y) && (contadorCeros_Y < 3) ) |
jjmedina | 0:a5367bd4e404 | 2646 | { |
jjmedina | 0:a5367bd4e404 | 2647 | if ( Yvibweighted > Yvibweighted_max ) |
jjmedina | 0:a5367bd4e404 | 2648 | { |
jjmedina | 0:a5367bd4e404 | 2649 | Yvibweighted_max = Yvibweighted; |
jjmedina | 0:a5367bd4e404 | 2650 | } |
jjmedina | 0:a5367bd4e404 | 2651 | if ( Yvibweighted < Yvibweighted_min ) |
jjmedina | 0:a5367bd4e404 | 2652 | { |
jjmedina | 0:a5367bd4e404 | 2653 | Yvibweighted_min = Yvibweighted; |
jjmedina | 0:a5367bd4e404 | 2654 | } |
jjmedina | 0:a5367bd4e404 | 2655 | }else if (contadorCeros_Y == 3) |
jjmedina | 0:a5367bd4e404 | 2656 | { |
jjmedina | 0:a5367bd4e404 | 2657 | contadorCeros_Y = 0; |
jjmedina | 0:a5367bd4e404 | 2658 | difY = (Yvibweighted_max - Yvibweighted_min); |
jjmedina | 0:a5367bd4e404 | 2659 | if ( difY > max_peaktopeak_Y_ ){max_peaktopeak_Y_ = difY;} |
jjmedina | 0:a5367bd4e404 | 2660 | //almacena para A95 |
jjmedina | 0:a5367bd4e404 | 2661 | fprintf(fp4,"%f \n", difY); |
jjmedina | 0:a5367bd4e404 | 2662 | contador_peaktopeak_Y++; |
jjmedina | 0:a5367bd4e404 | 2663 | Yvibweighted_max = 0.0; |
jjmedina | 0:a5367bd4e404 | 2664 | Yvibweighted_min = 0.0; |
jjmedina | 0:a5367bd4e404 | 2665 | } |
jjmedina | 0:a5367bd4e404 | 2666 | } |
jjmedina | 0:a5367bd4e404 | 2667 | } |
jjmedina | 0:a5367bd4e404 | 2668 | |
jjmedina | 0:a5367bd4e404 | 2669 | }//fin Loop Principal |
jjmedina | 0:a5367bd4e404 | 2670 | |
jjmedina | 0:a5367bd4e404 | 2671 | |
jjmedina | 0:a5367bd4e404 | 2672 | fclose(fp); |
jjmedina | 0:a5367bd4e404 | 2673 | if ( fp2 != NULL ) |
jjmedina | 0:a5367bd4e404 | 2674 | { |
jjmedina | 0:a5367bd4e404 | 2675 | fclose(fp2); |
jjmedina | 0:a5367bd4e404 | 2676 | } |
jjmedina | 0:a5367bd4e404 | 2677 | if ( fp3 != NULL ) |
jjmedina | 0:a5367bd4e404 | 2678 | { |
jjmedina | 0:a5367bd4e404 | 2679 | fclose(fp3); |
jjmedina | 0:a5367bd4e404 | 2680 | } |
jjmedina | 0:a5367bd4e404 | 2681 | if ( fp4 != NULL ) |
jjmedina | 0:a5367bd4e404 | 2682 | { |
jjmedina | 0:a5367bd4e404 | 2683 | fclose(fp4); |
jjmedina | 0:a5367bd4e404 | 2684 | } |
jjmedina | 0:a5367bd4e404 | 2685 | |
jjmedina | 0:a5367bd4e404 | 2686 | |
jjmedina | 0:a5367bd4e404 | 2687 | ///////////////////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 2688 | //Calculo A95s/////////////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 2689 | //Z |
jjmedina | 0:a5367bd4e404 | 2690 | //abro fichero |
jjmedina | 0:a5367bd4e404 | 2691 | |
jjmedina | 0:a5367bd4e404 | 2692 | fp2 = fopen(A95file,"r"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95file); } |
jjmedina | 0:a5367bd4e404 | 2693 | float maxZvibweighted; |
jjmedina | 0:a5367bd4e404 | 2694 | int length_maximoZvib = (int)(contador_peaktopeak_car*0.05) + 1; |
jjmedina | 0:a5367bd4e404 | 2695 | int maxindex(0); |
jjmedina | 0:a5367bd4e404 | 2696 | vector<float> maximoZvib (length_maximoZvib,0.0); |
jjmedina | 0:a5367bd4e404 | 2697 | vector<float> diferencia_vec; |
jjmedina | 0:a5367bd4e404 | 2698 | vector<float>::iterator up; |
jjmedina | 0:a5367bd4e404 | 2699 | float diferencia; |
jjmedina | 0:a5367bd4e404 | 2700 | float maximo; |
jjmedina | 0:a5367bd4e404 | 2701 | int contadorA95(0); |
jjmedina | 0:a5367bd4e404 | 2702 | //pasada 1 |
jjmedina | 0:a5367bd4e404 | 2703 | for(unsigned int i = 0; i<contador_peaktopeak_car; i++) |
jjmedina | 0:a5367bd4e404 | 2704 | { |
jjmedina | 0:a5367bd4e404 | 2705 | fscanf(fp2,"%f", &maxZvibweighted); |
jjmedina | 0:a5367bd4e404 | 2706 | for (unsigned int j = 0; j<length_maximoZvib; j++) |
jjmedina | 0:a5367bd4e404 | 2707 | { |
jjmedina | 0:a5367bd4e404 | 2708 | diferencia = maxZvibweighted - maximoZvib.at(j); |
jjmedina | 0:a5367bd4e404 | 2709 | if ( diferencia < 0 ) |
jjmedina | 0:a5367bd4e404 | 2710 | { |
jjmedina | 0:a5367bd4e404 | 2711 | diferencia_vec.push_back(0.0); |
jjmedina | 0:a5367bd4e404 | 2712 | contadorA95++; |
jjmedina | 0:a5367bd4e404 | 2713 | }else |
jjmedina | 0:a5367bd4e404 | 2714 | { |
jjmedina | 0:a5367bd4e404 | 2715 | diferencia_vec.push_back(diferencia); |
jjmedina | 0:a5367bd4e404 | 2716 | } |
jjmedina | 0:a5367bd4e404 | 2717 | } |
jjmedina | 0:a5367bd4e404 | 2718 | if ( contadorA95 == length_maximoZvib ){} |
jjmedina | 0:a5367bd4e404 | 2719 | else |
jjmedina | 0:a5367bd4e404 | 2720 | { |
jjmedina | 0:a5367bd4e404 | 2721 | maximo = *max_element(diferencia_vec.begin(),diferencia_vec.end()); |
jjmedina | 0:a5367bd4e404 | 2722 | up = find(diferencia_vec.begin(), diferencia_vec.end(), maximo); |
jjmedina | 0:a5367bd4e404 | 2723 | maxindex = int(up - diferencia_vec.begin()); |
jjmedina | 0:a5367bd4e404 | 2724 | maximoZvib.erase(maximoZvib.begin() + maxindex ); |
jjmedina | 0:a5367bd4e404 | 2725 | maximoZvib.insert(maximoZvib.begin() + maxindex ,maxZvibweighted); |
jjmedina | 0:a5367bd4e404 | 2726 | } |
jjmedina | 0:a5367bd4e404 | 2727 | contadorA95 = 0; |
jjmedina | 0:a5367bd4e404 | 2728 | diferencia_vec.clear(); |
jjmedina | 0:a5367bd4e404 | 2729 | } |
jjmedina | 0:a5367bd4e404 | 2730 | sort(maximoZvib.begin(),maximoZvib.end());//ascending |
jjmedina | 0:a5367bd4e404 | 2731 | reverse(maximoZvib.begin(),maximoZvib.end());//descending |
jjmedina | 0:a5367bd4e404 | 2732 | fclose(fp2); |
jjmedina | 0:a5367bd4e404 | 2733 | |
jjmedina | 0:a5367bd4e404 | 2734 | |
jjmedina | 0:a5367bd4e404 | 2735 | |
jjmedina | 0:a5367bd4e404 | 2736 | //pasada 2 |
jjmedina | 0:a5367bd4e404 | 2737 | //abro fichero |
jjmedina | 0:a5367bd4e404 | 2738 | fp2 = fopen(A95file,"r"); if(fp2 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95file); } |
jjmedina | 0:a5367bd4e404 | 2739 | double resta(1000.0); |
jjmedina | 0:a5367bd4e404 | 2740 | for(unsigned int i = 0; i<contador_peaktopeak_car; i++) |
jjmedina | 0:a5367bd4e404 | 2741 | { |
jjmedina | 0:a5367bd4e404 | 2742 | fscanf(fp2,"%f", &maxZvibweighted); |
jjmedina | 0:a5367bd4e404 | 2743 | if( ((maximoZvib.at(maximoZvib.size() - 1) - maxZvibweighted) <= resta) && (maxZvibweighted < maximoZvib.at(maximoZvib.size() - 1)) ) |
jjmedina | 0:a5367bd4e404 | 2744 | { |
jjmedina | 0:a5367bd4e404 | 2745 | A95_peaktopeak_car_ = maxZvibweighted; |
jjmedina | 0:a5367bd4e404 | 2746 | resta = maximoZvib.at(maximoZvib.size() - 1) - maxZvibweighted; |
jjmedina | 0:a5367bd4e404 | 2747 | |
jjmedina | 0:a5367bd4e404 | 2748 | } |
jjmedina | 0:a5367bd4e404 | 2749 | } |
jjmedina | 0:a5367bd4e404 | 2750 | //clear all vectors |
jjmedina | 0:a5367bd4e404 | 2751 | maximoZvib.clear(); |
jjmedina | 0:a5367bd4e404 | 2752 | diferencia_vec.clear(); |
jjmedina | 0:a5367bd4e404 | 2753 | |
jjmedina | 0:a5367bd4e404 | 2754 | |
jjmedina | 0:a5367bd4e404 | 2755 | |
jjmedina | 0:a5367bd4e404 | 2756 | fclose(fp2); |
jjmedina | 0:a5367bd4e404 | 2757 | |
jjmedina | 0:a5367bd4e404 | 2758 | //X///////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 2759 | //abro fichero |
jjmedina | 0:a5367bd4e404 | 2760 | fp3 = fopen(A95fileX,"r"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileX); } |
jjmedina | 0:a5367bd4e404 | 2761 | float maxXvibweighted; |
jjmedina | 0:a5367bd4e404 | 2762 | int length_maximoXvib = (int)(contador_peaktopeak_X*0.05) + 1; |
jjmedina | 0:a5367bd4e404 | 2763 | maxindex = 0; |
jjmedina | 0:a5367bd4e404 | 2764 | vector<float> maximoXvib (length_maximoXvib,0.0); |
jjmedina | 0:a5367bd4e404 | 2765 | diferencia = 0.0; |
jjmedina | 0:a5367bd4e404 | 2766 | maximo =0.0; |
jjmedina | 0:a5367bd4e404 | 2767 | contadorA95 = 0; |
jjmedina | 0:a5367bd4e404 | 2768 | if ( fp3 == NULL ) |
jjmedina | 0:a5367bd4e404 | 2769 | { |
jjmedina | 0:a5367bd4e404 | 2770 | A95_peaktopeak_X_ = 0.0; |
jjmedina | 0:a5367bd4e404 | 2771 | }else |
jjmedina | 0:a5367bd4e404 | 2772 | { |
jjmedina | 0:a5367bd4e404 | 2773 | //pasada 1 |
jjmedina | 0:a5367bd4e404 | 2774 | for(unsigned int i = 0; i<contador_peaktopeak_X; i++) |
jjmedina | 0:a5367bd4e404 | 2775 | { |
jjmedina | 0:a5367bd4e404 | 2776 | fscanf(fp3,"%f", &maxXvibweighted); |
jjmedina | 0:a5367bd4e404 | 2777 | for (unsigned int j = 0; j<length_maximoXvib; j++) |
jjmedina | 0:a5367bd4e404 | 2778 | { |
jjmedina | 0:a5367bd4e404 | 2779 | diferencia = maxXvibweighted - maximoXvib.at(j); |
jjmedina | 0:a5367bd4e404 | 2780 | if ( diferencia < 0 ) |
jjmedina | 0:a5367bd4e404 | 2781 | { |
jjmedina | 0:a5367bd4e404 | 2782 | diferencia_vec.push_back(0.0); |
jjmedina | 0:a5367bd4e404 | 2783 | contadorA95++; |
jjmedina | 0:a5367bd4e404 | 2784 | }else |
jjmedina | 0:a5367bd4e404 | 2785 | { |
jjmedina | 0:a5367bd4e404 | 2786 | diferencia_vec.push_back(diferencia); |
jjmedina | 0:a5367bd4e404 | 2787 | } |
jjmedina | 0:a5367bd4e404 | 2788 | } |
jjmedina | 0:a5367bd4e404 | 2789 | if ( contadorA95 == length_maximoXvib ){} |
jjmedina | 0:a5367bd4e404 | 2790 | else |
jjmedina | 0:a5367bd4e404 | 2791 | { |
jjmedina | 0:a5367bd4e404 | 2792 | maximo = *max_element(diferencia_vec.begin(),diferencia_vec.end()); |
jjmedina | 0:a5367bd4e404 | 2793 | up = find(diferencia_vec.begin(), diferencia_vec.end(), maximo); |
jjmedina | 0:a5367bd4e404 | 2794 | maxindex = int(up - diferencia_vec.begin()); |
jjmedina | 0:a5367bd4e404 | 2795 | maximoXvib.erase(maximoXvib.begin() + maxindex ); |
jjmedina | 0:a5367bd4e404 | 2796 | maximoXvib.insert(maximoXvib.begin() + maxindex ,maxXvibweighted); |
jjmedina | 0:a5367bd4e404 | 2797 | } |
jjmedina | 0:a5367bd4e404 | 2798 | contadorA95 = 0; |
jjmedina | 0:a5367bd4e404 | 2799 | diferencia_vec.clear(); |
jjmedina | 0:a5367bd4e404 | 2800 | } |
jjmedina | 0:a5367bd4e404 | 2801 | sort(maximoXvib.begin(),maximoXvib.end());//ascending |
jjmedina | 0:a5367bd4e404 | 2802 | reverse(maximoXvib.begin(),maximoXvib.end());//descending |
jjmedina | 0:a5367bd4e404 | 2803 | fclose(fp3); |
jjmedina | 0:a5367bd4e404 | 2804 | |
jjmedina | 0:a5367bd4e404 | 2805 | //pasada 2 |
jjmedina | 0:a5367bd4e404 | 2806 | //abro fichero |
jjmedina | 0:a5367bd4e404 | 2807 | fp3 = fopen(A95fileX,"r"); if(fp3 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileX); } |
jjmedina | 0:a5367bd4e404 | 2808 | resta = 1000.0; |
jjmedina | 0:a5367bd4e404 | 2809 | for(unsigned int i = 0; i<contador_peaktopeak_X; i++) |
jjmedina | 0:a5367bd4e404 | 2810 | { |
jjmedina | 0:a5367bd4e404 | 2811 | fscanf(fp3,"%f", &maxXvibweighted); |
jjmedina | 0:a5367bd4e404 | 2812 | if( ((maximoXvib.at(maximoXvib.size() - 1) - maxXvibweighted) <= resta) && (maxXvibweighted < maximoXvib.at(maximoXvib.size() - 1)) ) |
jjmedina | 0:a5367bd4e404 | 2813 | { |
jjmedina | 0:a5367bd4e404 | 2814 | A95_peaktopeak_X_ = maxXvibweighted; |
jjmedina | 0:a5367bd4e404 | 2815 | resta = maximoXvib.at(maximoXvib.size() - 1) - maxXvibweighted; |
jjmedina | 0:a5367bd4e404 | 2816 | |
jjmedina | 0:a5367bd4e404 | 2817 | } |
jjmedina | 0:a5367bd4e404 | 2818 | } |
jjmedina | 0:a5367bd4e404 | 2819 | //clear all vectors |
jjmedina | 0:a5367bd4e404 | 2820 | maximoXvib.clear(); |
jjmedina | 0:a5367bd4e404 | 2821 | diferencia_vec.clear(); |
jjmedina | 0:a5367bd4e404 | 2822 | } |
jjmedina | 0:a5367bd4e404 | 2823 | |
jjmedina | 0:a5367bd4e404 | 2824 | |
jjmedina | 0:a5367bd4e404 | 2825 | |
jjmedina | 0:a5367bd4e404 | 2826 | fclose(fp3); |
jjmedina | 0:a5367bd4e404 | 2827 | |
jjmedina | 0:a5367bd4e404 | 2828 | //Y///////////////////////////////////////// |
jjmedina | 0:a5367bd4e404 | 2829 | //abro fichero |
jjmedina | 0:a5367bd4e404 | 2830 | fp4 = fopen(A95fileY,"r"); if(fp4 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileY); } |
jjmedina | 0:a5367bd4e404 | 2831 | float maxYvibweighted; |
jjmedina | 0:a5367bd4e404 | 2832 | int length_maximoYvib = (int)(contador_peaktopeak_Y*0.05) + 1; |
jjmedina | 0:a5367bd4e404 | 2833 | maxindex = 0; |
jjmedina | 0:a5367bd4e404 | 2834 | vector<float> maximoYvib (length_maximoYvib,0.0); |
jjmedina | 0:a5367bd4e404 | 2835 | diferencia = 0.0; |
jjmedina | 0:a5367bd4e404 | 2836 | maximo =0.0; |
jjmedina | 0:a5367bd4e404 | 2837 | contadorA95 = 0; |
jjmedina | 0:a5367bd4e404 | 2838 | if ( fp4 == NULL ) |
jjmedina | 0:a5367bd4e404 | 2839 | { |
jjmedina | 0:a5367bd4e404 | 2840 | A95_peaktopeak_Y_ = 0.0; |
jjmedina | 0:a5367bd4e404 | 2841 | }else |
jjmedina | 0:a5367bd4e404 | 2842 | { |
jjmedina | 0:a5367bd4e404 | 2843 | //pasada 1 |
jjmedina | 0:a5367bd4e404 | 2844 | for(unsigned int i = 0; i<contador_peaktopeak_Y; i++) |
jjmedina | 0:a5367bd4e404 | 2845 | { |
jjmedina | 0:a5367bd4e404 | 2846 | fscanf(fp4,"%f", &maxYvibweighted); |
jjmedina | 0:a5367bd4e404 | 2847 | for (unsigned int j = 0; j<length_maximoYvib; j++) |
jjmedina | 0:a5367bd4e404 | 2848 | { |
jjmedina | 0:a5367bd4e404 | 2849 | diferencia = maxYvibweighted - maximoYvib.at(j); |
jjmedina | 0:a5367bd4e404 | 2850 | if ( diferencia < 0 ) |
jjmedina | 0:a5367bd4e404 | 2851 | { |
jjmedina | 0:a5367bd4e404 | 2852 | diferencia_vec.push_back(0.0); |
jjmedina | 0:a5367bd4e404 | 2853 | contadorA95++; |
jjmedina | 0:a5367bd4e404 | 2854 | }else |
jjmedina | 0:a5367bd4e404 | 2855 | { |
jjmedina | 0:a5367bd4e404 | 2856 | diferencia_vec.push_back(diferencia); |
jjmedina | 0:a5367bd4e404 | 2857 | } |
jjmedina | 0:a5367bd4e404 | 2858 | } |
jjmedina | 0:a5367bd4e404 | 2859 | if ( contadorA95 == length_maximoYvib ){} |
jjmedina | 0:a5367bd4e404 | 2860 | else |
jjmedina | 0:a5367bd4e404 | 2861 | { |
jjmedina | 0:a5367bd4e404 | 2862 | maximo = *max_element(diferencia_vec.begin(),diferencia_vec.end()); |
jjmedina | 0:a5367bd4e404 | 2863 | up = find(diferencia_vec.begin(), diferencia_vec.end(), maximo); |
jjmedina | 0:a5367bd4e404 | 2864 | maxindex = int(up - diferencia_vec.begin()); |
jjmedina | 0:a5367bd4e404 | 2865 | maximoYvib.erase(maximoYvib.begin() + maxindex ); |
jjmedina | 0:a5367bd4e404 | 2866 | maximoYvib.insert(maximoYvib.begin() + maxindex ,maxYvibweighted); |
jjmedina | 0:a5367bd4e404 | 2867 | } |
jjmedina | 0:a5367bd4e404 | 2868 | contadorA95 = 0; |
jjmedina | 0:a5367bd4e404 | 2869 | diferencia_vec.clear(); |
jjmedina | 0:a5367bd4e404 | 2870 | } |
jjmedina | 0:a5367bd4e404 | 2871 | sort(maximoYvib.begin(),maximoYvib.end());//ascending |
jjmedina | 0:a5367bd4e404 | 2872 | reverse(maximoYvib.begin(),maximoYvib.end());//descending |
jjmedina | 0:a5367bd4e404 | 2873 | fclose(fp4); |
jjmedina | 0:a5367bd4e404 | 2874 | |
jjmedina | 0:a5367bd4e404 | 2875 | //pasada 2 |
jjmedina | 0:a5367bd4e404 | 2876 | //abro fichero |
jjmedina | 0:a5367bd4e404 | 2877 | fp4 = fopen(A95fileY,"r"); if(fp4 == NULL) { display.Clear();display.SetXY(0,0); display.printf("Could not open file %s for write\n", A95fileY); } |
jjmedina | 0:a5367bd4e404 | 2878 | resta = 1000.0; |
jjmedina | 0:a5367bd4e404 | 2879 | for(unsigned int i = 0; i<contador_peaktopeak_Y; i++) |
jjmedina | 0:a5367bd4e404 | 2880 | { |
jjmedina | 0:a5367bd4e404 | 2881 | fscanf(fp4,"%f", &maxYvibweighted); |
jjmedina | 0:a5367bd4e404 | 2882 | if( ((maximoYvib.at(maximoYvib.size() - 1) - maxYvibweighted) <= resta) && (maxYvibweighted < maximoYvib.at(maximoYvib.size() - 1)) ) |
jjmedina | 0:a5367bd4e404 | 2883 | { |
jjmedina | 0:a5367bd4e404 | 2884 | A95_peaktopeak_Y_ = maxYvibweighted; |
jjmedina | 0:a5367bd4e404 | 2885 | resta = maximoYvib.at(maximoYvib.size() - 1) - maxYvibweighted; |
jjmedina | 0:a5367bd4e404 | 2886 | |
jjmedina | 0:a5367bd4e404 | 2887 | } |
jjmedina | 0:a5367bd4e404 | 2888 | } |
jjmedina | 0:a5367bd4e404 | 2889 | //clear all vectors |
jjmedina | 0:a5367bd4e404 | 2890 | maximoYvib.clear(); |
jjmedina | 0:a5367bd4e404 | 2891 | diferencia_vec.clear(); |
jjmedina | 0:a5367bd4e404 | 2892 | } |
jjmedina | 0:a5367bd4e404 | 2893 | |
jjmedina | 0:a5367bd4e404 | 2894 | |
jjmedina | 0:a5367bd4e404 | 2895 | fclose(fp4); |
jjmedina | 0:a5367bd4e404 | 2896 | //remove files |
jjmedina | 0:a5367bd4e404 | 2897 | int rmDVA0 = remove(A95file); if(rmDVA0 != 0) { display.Clear();display.SetXY(0,0); display.printf("Error removing %s file\n", A95file); } |
jjmedina | 0:a5367bd4e404 | 2898 | |
jjmedina | 0:a5367bd4e404 | 2899 | int rmDVA1 = remove(A95fileX); if(rmDVA1 != 0) { display.Clear();display.SetXY(0,0); display.printf("Error removing %s file\n", A95fileX); } |
jjmedina | 0:a5367bd4e404 | 2900 | int rmDVA2 = remove(A95fileY); if(rmDVA2 != 0) { display.Clear();display.SetXY(0,0); display.printf("Error removing %s file\n", A95fileY); } |
jjmedina | 0:a5367bd4e404 | 2901 | int rmDVA3 = remove(dispVelFile); if(rmDVA3 != 0) { display.Clear();display.SetXY(0,0); display.printf("Error removing %s file\n", dispVelFile); } |
jjmedina | 0:a5367bd4e404 | 2902 | |
jjmedina | 0:a5367bd4e404 | 2903 | |
jjmedina | 0:a5367bd4e404 | 2904 | |
jjmedina | 0:a5367bd4e404 | 2905 | |
jjmedina | 0:a5367bd4e404 | 2906 | |
jjmedina | 0:a5367bd4e404 | 2907 | |
jjmedina | 0:a5367bd4e404 | 2908 | |
jjmedina | 0:a5367bd4e404 | 2909 | |
jjmedina | 0:a5367bd4e404 | 2910 | return; |
jjmedina | 0:a5367bd4e404 | 2911 | |
jjmedina | 0:a5367bd4e404 | 2912 | } |
jjmedina | 0:a5367bd4e404 | 2913 | |
jjmedina | 0:a5367bd4e404 | 2914 | float RegresionJerk(float * aZ, unsigned int ndat) |
jjmedina | 0:a5367bd4e404 | 2915 | { |
jjmedina | 0:a5367bd4e404 | 2916 | //Media de las aceleraciones |
jjmedina | 0:a5367bd4e404 | 2917 | float aZm = 0.0; |
jjmedina | 0:a5367bd4e404 | 2918 | for(unsigned int j = 0; j<ndat; j++) { aZm += aZ[j]; } |
jjmedina | 0:a5367bd4e404 | 2919 | aZm = aZm/(float)ndat; |
jjmedina | 0:a5367bd4e404 | 2920 | |
jjmedina | 0:a5367bd4e404 | 2921 | //Pendiente por minimos cuadrados |
jjmedina | 0:a5367bd4e404 | 2922 | float b; |
jjmedina | 0:a5367bd4e404 | 2923 | //Numerador y denominador en expresion de pendiente |
jjmedina | 0:a5367bd4e404 | 2924 | float numer = 0.0; |
jjmedina | 0:a5367bd4e404 | 2925 | float denom = 0.0; |
jjmedina | 0:a5367bd4e404 | 2926 | |
jjmedina | 0:a5367bd4e404 | 2927 | int nn; |
jjmedina | 0:a5367bd4e404 | 2928 | float At; |
jjmedina | 0:a5367bd4e404 | 2929 | |
jjmedina | 0:a5367bd4e404 | 2930 | for(int j = 0; j<ndat; j++) |
jjmedina | 0:a5367bd4e404 | 2931 | { |
jjmedina | 0:a5367bd4e404 | 2932 | nn = j-((ndat/2)); |
jjmedina | 0:a5367bd4e404 | 2933 | At = (float)nn*(1.0e-6)*(float)(PasoTiempo); |
jjmedina | 0:a5367bd4e404 | 2934 | denom = denom + At*At; |
jjmedina | 0:a5367bd4e404 | 2935 | numer = numer + At*(aZ[j]-aZm); |
jjmedina | 0:a5367bd4e404 | 2936 | } |
jjmedina | 0:a5367bd4e404 | 2937 | b = numer/denom; |
jjmedina | 0:a5367bd4e404 | 2938 | |
jjmedina | 0:a5367bd4e404 | 2939 | return b; |
jjmedina | 0:a5367bd4e404 | 2940 | } |
jjmedina | 0:a5367bd4e404 | 2941 | |
jjmedina | 0:a5367bd4e404 | 2942 | void DisplayResultados1(void) |
jjmedina | 0:a5367bd4e404 | 2943 | { |
jjmedina | 0:a5367bd4e404 | 2944 | for(int i = 0; i<2; i++) |
jjmedina | 0:a5367bd4e404 | 2945 | { |
jjmedina | 0:a5367bd4e404 | 2946 | |
jjmedina | 0:a5367bd4e404 | 2947 | display.Clear(); |
jjmedina | 0:a5367bd4e404 | 2948 | if(TRADUCE==0) |
jjmedina | 0:a5367bd4e404 | 2949 | { |
jjmedina | 0:a5367bd4e404 | 2950 | display.SetXY(0,0); |
jjmedina | 0:a5367bd4e404 | 2951 | display.printf("Datos velocidad"); |
jjmedina | 0:a5367bd4e404 | 2952 | display.SetXY(0,1); |
jjmedina | 0:a5367bd4e404 | 2953 | display.printf("aceleracion jerk"); |
jjmedina | 0:a5367bd4e404 | 2954 | }else |
jjmedina | 0:a5367bd4e404 | 2955 | { |
jjmedina | 0:a5367bd4e404 | 2956 | display.SetXY(0,0); |
jjmedina | 0:a5367bd4e404 | 2957 | display.printf("Velocity jerk"); |
jjmedina | 0:a5367bd4e404 | 2958 | display.SetXY(0,1); |
jjmedina | 0:a5367bd4e404 | 2959 | display.printf("acceleration"); |
jjmedina | 0:a5367bd4e404 | 2960 | } |
jjmedina | 0:a5367bd4e404 | 2961 | |
jjmedina | 0:a5367bd4e404 | 2962 | wait(2.0); |
jjmedina | 0:a5367bd4e404 | 2963 | |
jjmedina | 0:a5367bd4e404 | 2964 | display.Clear(); |
jjmedina | 0:a5367bd4e404 | 2965 | display.SetXY(0,0); |
jjmedina | 0:a5367bd4e404 | 2966 | display.printf("VNom = %4.2f m/s \n",vNom); |
jjmedina | 0:a5367bd4e404 | 2967 | display.SetXY(0,1); |
jjmedina | 0:a5367bd4e404 | 2968 | display.printf("Vmax = %4.2f m/s \n", Vmax); |
jjmedina | 0:a5367bd4e404 | 2969 | |
jjmedina | 0:a5367bd4e404 | 2970 | wait(2.0); |
jjmedina | 0:a5367bd4e404 | 2971 | |
jjmedina | 0:a5367bd4e404 | 2972 | display.Clear(); |
jjmedina | 0:a5367bd4e404 | 2973 | display.SetXY(0,0); |
jjmedina | 0:a5367bd4e404 | 2974 | display.printf("amax = %4.2f m/s2\n", amax); |
jjmedina | 0:a5367bd4e404 | 2975 | display.SetXY(0,1); |
jjmedina | 0:a5367bd4e404 | 2976 | display.printf("dmax = %4.2f m/s2\n", dmax); |
jjmedina | 0:a5367bd4e404 | 2977 | |
jjmedina | 0:a5367bd4e404 | 2978 | wait(2.0); |
jjmedina | 0:a5367bd4e404 | 2979 | |
jjmedina | 0:a5367bd4e404 | 2980 | display.Clear(); |
jjmedina | 0:a5367bd4e404 | 2981 | display.SetXY(0,0); |
jjmedina | 0:a5367bd4e404 | 2982 | display.printf("jmax = %4.2f m/s3\n", jerkmax); |
jjmedina | 0:a5367bd4e404 | 2983 | display.SetXY(0,1); |
jjmedina | 0:a5367bd4e404 | 2984 | display.printf("VA95 = %4.2f m/s \n", VA95); |
jjmedina | 0:a5367bd4e404 | 2985 | |
jjmedina | 0:a5367bd4e404 | 2986 | wait(2.0); |
jjmedina | 0:a5367bd4e404 | 2987 | |
jjmedina | 0:a5367bd4e404 | 2988 | display.Clear(); |
jjmedina | 0:a5367bd4e404 | 2989 | display.SetXY(0,0); |
jjmedina | 0:a5367bd4e404 | 2990 | display.printf("aA95 = %4.2f m/s2\n", aA95); |
jjmedina | 0:a5367bd4e404 | 2991 | display.SetXY(0,1); |
jjmedina | 0:a5367bd4e404 | 2992 | display.printf("dA95 = %4.2f m/s2\n", dA95); |
jjmedina | 0:a5367bd4e404 | 2993 | |
jjmedina | 0:a5367bd4e404 | 2994 | wait(2.0); |
jjmedina | 0:a5367bd4e404 | 2995 | |
jjmedina | 0:a5367bd4e404 | 2996 | } |
jjmedina | 0:a5367bd4e404 | 2997 | } |
jjmedina | 0:a5367bd4e404 | 2998 | |
jjmedina | 0:a5367bd4e404 | 2999 | void DisplayResultados2(void) |
jjmedina | 0:a5367bd4e404 | 3000 | { |
jjmedina | 0:a5367bd4e404 | 3001 | for(int i = 0; i<2; i++) |
jjmedina | 0:a5367bd4e404 | 3002 | { |
jjmedina | 0:a5367bd4e404 | 3003 | |
jjmedina | 0:a5367bd4e404 | 3004 | display.Clear(); |
jjmedina | 0:a5367bd4e404 | 3005 | if(TRADUCE==0) |
jjmedina | 0:a5367bd4e404 | 3006 | { |
jjmedina | 0:a5367bd4e404 | 3007 | display.SetXY(0,0); |
jjmedina | 0:a5367bd4e404 | 3008 | display.printf("Datos confort"); |
jjmedina | 0:a5367bd4e404 | 3009 | display.SetXY(0,0); |
jjmedina | 0:a5367bd4e404 | 3010 | display.printf("vibracion Z,X,Y:"); |
jjmedina | 0:a5367bd4e404 | 3011 | }else |
jjmedina | 0:a5367bd4e404 | 3012 | { |
jjmedina | 0:a5367bd4e404 | 3013 | display.SetXY(0,0); |
jjmedina | 0:a5367bd4e404 | 3014 | display.printf("Comfort info"); |
jjmedina | 0:a5367bd4e404 | 3015 | display.SetXY(0,0); |
jjmedina | 0:a5367bd4e404 | 3016 | display.printf("vibration Z,X,Y:"); |
jjmedina | 0:a5367bd4e404 | 3017 | } |
jjmedina | 0:a5367bd4e404 | 3018 | wait(2.0); |
jjmedina | 0:a5367bd4e404 | 3019 | |
jjmedina | 0:a5367bd4e404 | 3020 | |
jjmedina | 0:a5367bd4e404 | 3021 | |
jjmedina | 0:a5367bd4e404 | 3022 | display.Clear(); |
jjmedina | 0:a5367bd4e404 | 3023 | display.SetXY(0,0); |
jjmedina | 0:a5367bd4e404 | 3024 | display.printf("Zmaxca=%4.2f m/s2 \n", max_peaktopeak_car_); |
jjmedina | 0:a5367bd4e404 | 3025 | display.SetXY(0,1); |
jjmedina | 0:a5367bd4e404 | 3026 | display.printf("ZA95ca=%4.2f m/s2 \n", A95_peaktopeak_car_); |
jjmedina | 0:a5367bd4e404 | 3027 | |
jjmedina | 0:a5367bd4e404 | 3028 | wait(2.0); |
jjmedina | 0:a5367bd4e404 | 3029 | |
jjmedina | 0:a5367bd4e404 | 3030 | display.Clear(); |
jjmedina | 0:a5367bd4e404 | 3031 | display.SetXY(0,0); |
jjmedina | 0:a5367bd4e404 | 3032 | display.printf("Zmaxnca=%4.2f m/s2 \n", max_peaktopeak_ncar_); |
jjmedina | 0:a5367bd4e404 | 3033 | |
jjmedina | 0:a5367bd4e404 | 3034 | wait(2.0); |
jjmedina | 0:a5367bd4e404 | 3035 | |
jjmedina | 0:a5367bd4e404 | 3036 | display.Clear(); |
jjmedina | 0:a5367bd4e404 | 3037 | display.SetXY(0,0); |
jjmedina | 0:a5367bd4e404 | 3038 | display.printf("Xmax = %4.2f m/s2\n", max_peaktopeak_X_); |
jjmedina | 0:a5367bd4e404 | 3039 | display.SetXY(0,1); |
jjmedina | 0:a5367bd4e404 | 3040 | display.printf("XA95 = %4.2f m/s2\n", A95_peaktopeak_X_); |
jjmedina | 0:a5367bd4e404 | 3041 | |
jjmedina | 0:a5367bd4e404 | 3042 | wait(2.0); |
jjmedina | 0:a5367bd4e404 | 3043 | |
jjmedina | 0:a5367bd4e404 | 3044 | display.Clear(); |
jjmedina | 0:a5367bd4e404 | 3045 | display.SetXY(0,0); |
jjmedina | 0:a5367bd4e404 | 3046 | display.printf("Ymax = %4.2f m/s2\n", max_peaktopeak_Y_); |
jjmedina | 0:a5367bd4e404 | 3047 | display.SetXY(0,1); |
jjmedina | 0:a5367bd4e404 | 3048 | display.printf("YA95 = %4.2f m/s2\n", A95_peaktopeak_Y_); |
jjmedina | 0:a5367bd4e404 | 3049 | |
jjmedina | 0:a5367bd4e404 | 3050 | wait(2.0); |
jjmedina | 0:a5367bd4e404 | 3051 | } |
jjmedina | 0:a5367bd4e404 | 3052 | } |
jjmedina | 0:a5367bd4e404 | 3053 | |
jjmedina | 0:a5367bd4e404 | 3054 | void FileResultados( unsigned int ContadorEnsayos ) |
jjmedina | 0:a5367bd4e404 | 3055 | { |
jjmedina | 0:a5367bd4e404 | 3056 | FILE * fp; |
jjmedina | 0:a5367bd4e404 | 3057 | |
jjmedina | 0:a5367bd4e404 | 3058 | char NoViaje[2]; |
jjmedina | 0:a5367bd4e404 | 3059 | char resultsFile[50]; |
jjmedina | 0:a5367bd4e404 | 3060 | |
jjmedina | 0:a5367bd4e404 | 3061 | strcpy(resultsFile,"/local/ISO_"); |
jjmedina | 0:a5367bd4e404 | 3062 | |
jjmedina | 0:a5367bd4e404 | 3063 | sprintf (NoViaje, "%d", ContadorEnsayos); |
jjmedina | 0:a5367bd4e404 | 3064 | strcat(resultsFile,NoViaje); |
jjmedina | 0:a5367bd4e404 | 3065 | strcat(resultsFile,".txt"); |
jjmedina | 0:a5367bd4e404 | 3066 | |
jjmedina | 0:a5367bd4e404 | 3067 | |
jjmedina | 0:a5367bd4e404 | 3068 | fp = fopen(resultsFile,"w"); if(fp == NULL) { display.printf("Could not open file %s for read\n", resultsFile); } |
jjmedina | 0:a5367bd4e404 | 3069 | 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_); |
jjmedina | 0:a5367bd4e404 | 3070 | //fprintf(fp,"%d %d %d \n", Bo1, Bo2, Bo3); |
jjmedina | 0:a5367bd4e404 | 3071 | fclose(fp); |
jjmedina | 0:a5367bd4e404 | 3072 | |
jjmedina | 0:a5367bd4e404 | 3073 | } |
jjmedina | 0:a5367bd4e404 | 3074 | |
jjmedina | 0:a5367bd4e404 | 3075 | void LeeAcelerometroBuffer() |
jjmedina | 0:a5367bd4e404 | 3076 | { |
jjmedina | 0:a5367bd4e404 | 3077 | bma180.ReadAccels_BMA180(readings); |
jjmedina | 0:a5367bd4e404 | 3078 | |
jjmedina | 0:a5367bd4e404 | 3079 | aaX = (int16_t)readings[0]; |
jjmedina | 0:a5367bd4e404 | 3080 | aaY = (int16_t)readings[1]; |
jjmedina | 0:a5367bd4e404 | 3081 | aaZ = (int16_t)readings[2]; |
jjmedina | 0:a5367bd4e404 | 3082 | //tt_inst = (int16_t)timer.read_us(); |
jjmedina | 0:a5367bd4e404 | 3083 | |
jjmedina | 0:a5367bd4e404 | 3084 | aX = (-1.0)*((float)aaX-(-103.23))*0.0018; |
jjmedina | 0:a5367bd4e404 | 3085 | aY = (-1.0)*((float)aaY-(57.21))*0.0018; |
jjmedina | 0:a5367bd4e404 | 3086 | aZ = (-1.0)*((float)aaZ-(701.1184))*0.0018097570403; |
jjmedina | 0:a5367bd4e404 | 3087 | |
jjmedina | 0:a5367bd4e404 | 3088 | //t_inst = (float)tt_inst; |
jjmedina | 0:a5367bd4e404 | 3089 | |
jjmedina | 0:a5367bd4e404 | 3090 | //buff_t.put(t_inst); |
jjmedina | 0:a5367bd4e404 | 3091 | buff_aX.put(aX); |
jjmedina | 0:a5367bd4e404 | 3092 | buff_aY.put(aY); |
jjmedina | 0:a5367bd4e404 | 3093 | buff_aZ.put(aZ); |
jjmedina | 0:a5367bd4e404 | 3094 | } |
jjmedina | 0:a5367bd4e404 | 3095 | |
jjmedina | 0:a5367bd4e404 | 3096 | void GuardaDatosBuffer() |
jjmedina | 0:a5367bd4e404 | 3097 | { |
jjmedina | 0:a5367bd4e404 | 3098 | bool empty = buff_aX.isEmpty(); |
jjmedina | 0:a5367bd4e404 | 3099 | int wi = buff_aX.getWritingIndex(); |
jjmedina | 0:a5367bd4e404 | 3100 | int ri = buff_aX.getReadingIndex(); |
jjmedina | 0:a5367bd4e404 | 3101 | |
jjmedina | 0:a5367bd4e404 | 3102 | int dif; //Diferencia entre writting index y riding index. El buffer se llena cuando esta diferencia alcanza el tamaño del buffer |
jjmedina | 0:a5367bd4e404 | 3103 | float tiempo; //Tiempo en segundos |
jjmedina | 0:a5367bd4e404 | 3104 | |
jjmedina | 0:a5367bd4e404 | 3105 | if((wi-ri)>0) { dif = wi-ri; } |
jjmedina | 0:a5367bd4e404 | 3106 | else { dif = sizeBuff+wi-ri; } |
jjmedina | 0:a5367bd4e404 | 3107 | |
jjmedina | 0:a5367bd4e404 | 3108 | if(empty == false) |
jjmedina | 0:a5367bd4e404 | 3109 | { |
jjmedina | 0:a5367bd4e404 | 3110 | aXr = buff_aX.get(); |
jjmedina | 0:a5367bd4e404 | 3111 | aYr = buff_aY.get(); |
jjmedina | 0:a5367bd4e404 | 3112 | aZr = buff_aZ.get(); |
jjmedina | 0:a5367bd4e404 | 3113 | |
jjmedina | 0:a5367bd4e404 | 3114 | tiempo = ((float)ContadorDatos)*((float)PasoTiempo)/1.0e6; |
jjmedina | 0:a5367bd4e404 | 3115 | |
jjmedina | 0:a5367bd4e404 | 3116 | //pc.printf("%d %f %f %f %d\n", ContadorDatos*PasoTiempo, aXr, aYr, aZr, dif); |
jjmedina | 0:a5367bd4e404 | 3117 | fprintf(fp,"%f %f %f %f %d\n", tiempo, aXr, aYr, aZr, dif); |
jjmedina | 0:a5367bd4e404 | 3118 | ContadorDatos++; |
jjmedina | 0:a5367bd4e404 | 3119 | } |
jjmedina | 0:a5367bd4e404 | 3120 | } |
jjmedina | 0:a5367bd4e404 | 3121 | |
jjmedina | 0:a5367bd4e404 | 3122 | void ParamDependFrecAdq() |
jjmedina | 0:a5367bd4e404 | 3123 | { |
jjmedina | 0:a5367bd4e404 | 3124 | |
jjmedina | 0:a5367bd4e404 | 3125 | if(FrecAdq == 320) |
jjmedina | 0:a5367bd4e404 | 3126 | { |
jjmedina | 0:a5367bd4e404 | 3127 | PasoTiempo = 3125; |
jjmedina | 0:a5367bd4e404 | 3128 | gain = 1.184456202e+02; |
jjmedina | 0:a5367bd4e404 | 3129 | c_butt1 = -0.7575469445; |
jjmedina | 0:a5367bd4e404 | 3130 | c_butt2 = 1.7237761728; |
jjmedina | 0:a5367bd4e404 | 3131 | ndes = 100; |
jjmedina | 0:a5367bd4e404 | 3132 | nmed = 180; |
jjmedina | 0:a5367bd4e404 | 3133 | } |
jjmedina | 0:a5367bd4e404 | 3134 | |
jjmedina | 0:a5367bd4e404 | 3135 | if(FrecAdq == 1000) |
jjmedina | 0:a5367bd4e404 | 3136 | { |
jjmedina | 0:a5367bd4e404 | 3137 | PasoTiempo = 1000; |
jjmedina | 0:a5367bd4e404 | 3138 | gain = 1.058546241e+03; |
jjmedina | 0:a5367bd4e404 | 3139 | c_butt1 = -0.9149758348; |
jjmedina | 0:a5367bd4e404 | 3140 | c_butt2 = 1.9111970674; |
jjmedina | 0:a5367bd4e404 | 3141 | ndes = 300; |
jjmedina | 0:a5367bd4e404 | 3142 | nmed = 600; |
jjmedina | 0:a5367bd4e404 | 3143 | } |
jjmedina | 0:a5367bd4e404 | 3144 | |
jjmedina | 0:a5367bd4e404 | 3145 | /// Para fadq = 100 Hz, PasoTiempo = 10000 /// |
jjmedina | 0:a5367bd4e404 | 3146 | |
jjmedina | 0:a5367bd4e404 | 3147 | //float gain = 1.482463775e+01; |
jjmedina | 0:a5367bd4e404 | 3148 | //float c_butt1 = -0.4128015981; |
jjmedina | 0:a5367bd4e404 | 3149 | //float c_butt2 = 1.1429805025; |
jjmedina | 0:a5367bd4e404 | 3150 | //unsigned int ndes = 30; |
jjmedina | 0:a5367bd4e404 | 3151 | //unsigned int nmed = 60; |
jjmedina | 0:a5367bd4e404 | 3152 | |
jjmedina | 0:a5367bd4e404 | 3153 | /// Para fadq = 328 Hz, PasoTiempo = 3050 /// |
jjmedina | 0:a5367bd4e404 | 3154 | |
jjmedina | 0:a5367bd4e404 | 3155 | //float gain = 1.239670776e+02; |
jjmedina | 0:a5367bd4e404 | 3156 | //float c_butt1 = -0.7626121550; |
jjmedina | 0:a5367bd4e404 | 3157 | //float c_butt2 = 1.7303455236; |
jjmedina | 0:a5367bd4e404 | 3158 | //unsigned int ndes = 100; |
jjmedina | 0:a5367bd4e404 | 3159 | //unsigned int nmed = 180; |
jjmedina | 0:a5367bd4e404 | 3160 | |
jjmedina | 0:a5367bd4e404 | 3161 | /// Para fadq = 320 Hz, PasoTiempo = 3125 /// |
jjmedina | 0:a5367bd4e404 | 3162 | |
jjmedina | 0:a5367bd4e404 | 3163 | //const float gain = 1.184456202e+02; |
jjmedina | 0:a5367bd4e404 | 3164 | //const float c_butt1 = -0.7575469445; |
jjmedina | 0:a5367bd4e404 | 3165 | //const float c_butt2 = 1.7237761728; |
jjmedina | 0:a5367bd4e404 | 3166 | //unsigned int ndes = 100; |
jjmedina | 0:a5367bd4e404 | 3167 | //unsigned int nmed = 180; |
jjmedina | 0:a5367bd4e404 | 3168 | |
jjmedina | 0:a5367bd4e404 | 3169 | /// Para fadq = 1000 Hz, PasoTiempo = 1000 /// |
jjmedina | 0:a5367bd4e404 | 3170 | |
jjmedina | 0:a5367bd4e404 | 3171 | //const float gain = 1.058546241e+03; |
jjmedina | 0:a5367bd4e404 | 3172 | //const float c_butt1 = -0.9149758348; |
jjmedina | 0:a5367bd4e404 | 3173 | //const float c_butt2 = 1.9111970674; |
jjmedina | 0:a5367bd4e404 | 3174 | //const unsigned int ndes = 300; |
jjmedina | 0:a5367bd4e404 | 3175 | //const unsigned int nmed = 600; |
jjmedina | 0:a5367bd4e404 | 3176 | } |