jorge Coss y Leon / Mbed 2 deprecated Afinador_Vfinal

final coss

Dependencies:   FastAnalogIn HSI2RGBW_PWM NVIC_set_all_priorities mbed-dsp mbed

Fork of Seniales-Final by Ricardo Soto

main.cpp@3:6c9dabbb7261, 2015-05-03 (annotated)

Committer:
Soto
Date:
Sun May 03 22:53:37 2015 +0000
Revision:
3:6c9dabbb7261
Parent:
2:035d551759a5
Child:
4:76ea7cc8fdbc
Proyecto de control de una silla de rueda mediante analisis FFT

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Soto 3:6c9dabbb7261 1 #include "mbed.h" //Librería default de mbed
Soto 3:6c9dabbb7261 2 #include "NVIC_set_all_priorities.h" //Librería para modificar todas las solicitudes de interrupciones al mismo tiempo
frankvnk 0:0c037aff5039 3 #include <ctype.h>
Soto 3:6c9dabbb7261 4 #include "arm_math.h" //Librería que contiene funciones de tranformada de fourier
frankvnk 2:035d551759a5 5 #include "arm_const_structs.h"
Soto 3:6c9dabbb7261 6 #include "FastAnalogIn.h" //Librería modificada basada en la función de AnalogIn que reduce el tiempo de procesamiento de las señales ADC
frankvnk 0:0c037aff5039 7
Soto 3:6c9dabbb7261 8 //**********Declaraciones devariables para la silla****************
frankvnk 0:0c037aff5039 9
Soto 3:6c9dabbb7261 10 DigitalOut EnableLeft(PTC9);//Salida que habilitara o deshabilitara el puente H de la llanta izquierda
Soto 3:6c9dabbb7261 11 DigitalOut EnableRight(PTC8);//Salida que habilitara o deshabilitara el puente H de la llanta derecha
frankvnk 0:0c037aff5039 12
Soto 3:6c9dabbb7261 13 DigitalOut RightControl1(PTA5);//Salidas que controlan los estados del puente H de la llanta derecha
Soto 3:6c9dabbb7261 14 DigitalOut RightControl2(PTA4);
frankvnk 0:0c037aff5039 15
Soto 3:6c9dabbb7261 16 DigitalOut LeftControl1(PTA12);//Salidas que controlan los estados del puente H de la llanta izquierda
Soto 3:6c9dabbb7261 17 DigitalOut LeftControl2(PTD4);
Soto 3:6c9dabbb7261 18 //*******************************************************
frankvnk 0:0c037aff5039 19
Soto 3:6c9dabbb7261 20 Serial pc(USBTX, USBRX); //Se declaran los pines que se utilizarán para la comunicación serial mediante USB para debuggeo (PTA1 - RX, PTA2 - TX)
Soto 3:6c9dabbb7261 21 FastAnalogIn segnal(PTC2); //Se declara el pin que recibirá la señal análoga del sensor
Soto 3:6c9dabbb7261 22
Soto 3:6c9dabbb7261 23 extern "C" void NMI_Handler() {
frankvnk 0:0c037aff5039 24 }
frankvnk 0:0c037aff5039 25
Soto 3:6c9dabbb7261 26 ////////////////////////////////////////////////////////////////////////////////
Soto 3:6c9dabbb7261 27 // CONFIGURACION
Soto 3:6c9dabbb7261 28 //Estos valores puedne modificarse para modificar los parametros de la transformada
Soto 3:6c9dabbb7261 29 ////////////////////////////////////////////////////////////////////////////////
frankvnk 0:0c037aff5039 30
Soto 3:6c9dabbb7261 31 int SAMPLE_RATE_HZ = 40000; // Frecuencian de muestreo en HZ del sistema
Soto 3:6c9dabbb7261 32 const int FFT_SIZE = 1024; // Número de valores para la transformada rápida
Soto 3:6c9dabbb7261 33 float freq = 40000.0/1024.0; // Frecuencia de activación de la interrupción de muestreo
Soto 3:6c9dabbb7261 34 float max[2]; // Arreglo que almacena la frecuencia y magnitud mayores del espectro de Fourier
Soto 3:6c9dabbb7261 35 char blue_freq=0; // Variable que almacena el valor que se envia por bluetooth de los BPM
frankvnk 0:0c037aff5039 36
frankvnk 0:0c037aff5039 37 ////////////////////////////////////////////////////////////////////////////////
Soto 3:6c9dabbb7261 38 // ESTADO INTERNO
Soto 3:6c9dabbb7261 39 // Configuraciones necesarias para el correcto funcionaiento del programa
Soto 3:6c9dabbb7261 40 ////////////////////////////////////////////////////////////////////////////////
frankvnk 0:0c037aff5039 41
Soto 3:6c9dabbb7261 42 const static arm_cfft_instance_f32 *S;
Soto 3:6c9dabbb7261 43 Ticker samplingTimer; //objeto creado para habilitar las interrupciones con lso métodos de ticker
Soto 3:6c9dabbb7261 44 float samples[FFT_SIZE*2]; //Arreglo en el que se almacenan las muestras del tomadas ADC
Soto 3:6c9dabbb7261 45 float magnitudes[FFT_SIZE]; //Arreglo donde se almacenan las magnitudes de la FFT
Soto 3:6c9dabbb7261 46 int sampleCounter = 0; //Contador del número de muestras tomadas
frankvnk 0:0c037aff5039 47
frankvnk 0:0c037aff5039 48 ////////////////////////////////////////////////////////////////////////////////
Soto 3:6c9dabbb7261 49 // FUNCIONES DE MUESTREO
frankvnk 0:0c037aff5039 50 ////////////////////////////////////////////////////////////////////////////////
frankvnk 0:0c037aff5039 51
Soto 3:6c9dabbb7261 52 //Esta función permite realizar el muestreo de datos, se realiza como interrupción para asegurar el tiempo de muestreo deseado
frankvnk 0:0c037aff5039 53 void samplingCallback()
frankvnk 0:0c037aff5039 54 {
Soto 3:6c9dabbb7261 55 // Lectura del ADC y almacenamiento del dato
Soto 3:6c9dabbb7261 56 samples[sampleCounter] = (1023 * segnal) - 511.0f; //Se ajusta el valor de un rango de 0-1 a 0-511
Soto 3:6c9dabbb7261 57 // La función que calcula la transformada requiere de un valor imaginario, en este caso se le asigna 0
Soto 3:6c9dabbb7261 58 // ya que los valores muestreados son solamente reales.
frankvnk 0:0c037aff5039 59 samples[sampleCounter+1] = 0.0;
Soto 3:6c9dabbb7261 60 // Se ajusta la posición en el arreglo para almacenar el siguiente valor real
frankvnk 0:0c037aff5039 61 sampleCounter += 2;
Soto 3:6c9dabbb7261 62 //En caso de que el valor de sample counter sobrepase el tamaño del arreglo de almacenamiento se retira la interrupción
Soto 3:6c9dabbb7261 63 //de muestreo del programa
frankvnk 0:0c037aff5039 64 if (sampleCounter >= FFT_SIZE*2) {
frankvnk 0:0c037aff5039 65 samplingTimer.detach();
frankvnk 0:0c037aff5039 66 }
frankvnk 0:0c037aff5039 67 }
frankvnk 0:0c037aff5039 68
Soto 3:6c9dabbb7261 69 //Esta función permite reiniciar el contador de muestras e insertar nuevamente la interrupción de muestreo
frankvnk 0:0c037aff5039 70 void samplingBegin()
frankvnk 0:0c037aff5039 71 {
Soto 3:6c9dabbb7261 72 sampleCounter = 0; //Se reinicia el contador de muestras
Soto 3:6c9dabbb7261 73 samplingTimer.attach_us(&samplingCallback, 1000000/SAMPLE_RATE_HZ); //Se incertala interrupción de muestreo la cual es llamada con la frecuencia de Sample_rate_hz
frankvnk 0:0c037aff5039 74 }
frankvnk 0:0c037aff5039 75
Soto 3:6c9dabbb7261 76 //Función booleana que funciona como bandera para indicar que el meustreo de datos ha sido finalizado
frankvnk 0:0c037aff5039 77 bool samplingIsDone()
frankvnk 0:0c037aff5039 78 {
frankvnk 0:0c037aff5039 79 return sampleCounter >= FFT_SIZE*2;
frankvnk 0:0c037aff5039 80 }
frankvnk 0:0c037aff5039 81
Soto 3:6c9dabbb7261 82 //************Funciones del funcionamiento de la silla
Soto 3:6c9dabbb7261 83 void Avanzar(void){//Esta funcion tiene las salidas programadas para que la silla avance hace delante
Soto 3:6c9dabbb7261 84 RightControl1=0;
Soto 3:6c9dabbb7261 85 RightControl2=1;
Soto 3:6c9dabbb7261 86 LeftControl1=0;
Soto 3:6c9dabbb7261 87 LeftControl2=1;
Soto 3:6c9dabbb7261 88 EnableLeft=1;
Soto 3:6c9dabbb7261 89 EnableRight=1;
Soto 3:6c9dabbb7261 90 }
Soto 3:6c9dabbb7261 91
Soto 3:6c9dabbb7261 92 void Retroceder(void){//Esta funcion las salidas programadas para que la silla retroceda
Soto 3:6c9dabbb7261 93 RightControl1=1;
Soto 3:6c9dabbb7261 94 RightControl2=0;
Soto 3:6c9dabbb7261 95 LeftControl1=1;
Soto 3:6c9dabbb7261 96 LeftControl2=0;
Soto 3:6c9dabbb7261 97 EnableLeft=1;
Soto 3:6c9dabbb7261 98 EnableRight=1;
Soto 3:6c9dabbb7261 99 }
Soto 3:6c9dabbb7261 100 void Derecha(void){//Esta funcion tiene las salidas programadas para que la silla avance a la derehca (apaga el motor derecho y enciende el motor izquierdo)
Soto 3:6c9dabbb7261 101 LeftControl1=0;
Soto 3:6c9dabbb7261 102 LeftControl2=1;
Soto 3:6c9dabbb7261 103 EnableLeft=1;
Soto 3:6c9dabbb7261 104 EnableRight=0;
Soto 3:6c9dabbb7261 105 }
Soto 3:6c9dabbb7261 106 void Izquierda(void){//Esta funcion tiene las salidas programadas para que la silla avance a la izquierda (apaga el motor izquierdo y enciende el motor derecho)
Soto 3:6c9dabbb7261 107 RightControl1=0;
Soto 3:6c9dabbb7261 108 RightControl2=1;
Soto 3:6c9dabbb7261 109 EnableLeft=0;
Soto 3:6c9dabbb7261 110 EnableRight=1;
Soto 3:6c9dabbb7261 111 }
Soto 3:6c9dabbb7261 112 void Alto(void){//Esta fucnion apaga los enables del puente H y por lo tanto la silla para
Soto 3:6c9dabbb7261 113 EnableLeft=0;
Soto 3:6c9dabbb7261 114 EnableRight=0;
Soto 3:6c9dabbb7261 115 }
Soto 3:6c9dabbb7261 116 //************************************************
frankvnk 0:0c037aff5039 117
frankvnk 0:0c037aff5039 118 ////////////////////////////////////////////////////////////////////////////////
Soto 3:6c9dabbb7261 119 // MAIN DEL PROGRAMA
frankvnk 0:0c037aff5039 120 ////////////////////////////////////////////////////////////////////////////////
frankvnk 0:0c037aff5039 121
frankvnk 0:0c037aff5039 122 int main()
frankvnk 0:0c037aff5039 123 {
Soto 3:6c9dabbb7261 124 //Configuración de las solicitudes de interrupción
frankvnk 0:0c037aff5039 125 NVIC_set_all_irq_priorities(1);
Soto 3:6c9dabbb7261 126
Soto 3:6c9dabbb7261 127 //Configuración de la velocidad de la comunicación serial
Soto 3:6c9dabbb7261 128 pc.baud (38400); //Velocidad de la comunicación USB
frankvnk 0:0c037aff5039 129
Soto 3:6c9dabbb7261 130 // Se incerta la interrupción de muestreo del ADC
frankvnk 0:0c037aff5039 131 samplingBegin();
frankvnk 0:0c037aff5039 132
Soto 3:6c9dabbb7261 133 // Init arm_ccft_32 el registro cambiara dependiendo de la variable FFT_SIZE
frankvnk 2:035d551759a5 134 switch (FFT_SIZE)
frankvnk 2:035d551759a5 135 {
frankvnk 2:035d551759a5 136 case 512:
frankvnk 2:035d551759a5 137 S = & arm_cfft_sR_f32_len512;
frankvnk 2:035d551759a5 138 break;
frankvnk 2:035d551759a5 139 case 1024:
frankvnk 2:035d551759a5 140 S = & arm_cfft_sR_f32_len1024;
frankvnk 2:035d551759a5 141 break;
frankvnk 2:035d551759a5 142 case 2048:
frankvnk 2:035d551759a5 143 S = & arm_cfft_sR_f32_len2048;
frankvnk 2:035d551759a5 144 break;
frankvnk 2:035d551759a5 145 case 4096:
frankvnk 2:035d551759a5 146 S = & arm_cfft_sR_f32_len4096;
frankvnk 2:035d551759a5 147 break;
frankvnk 2:035d551759a5 148 }
frankvnk 2:035d551759a5 149
frankvnk 0:0c037aff5039 150 while(1) {
Soto 3:6c9dabbb7261 151 // Se calcula la FFT si se ha terminado el muestreo
frankvnk 0:0c037aff5039 152 if (samplingIsDone()) {
Soto 3:6c9dabbb7261 153
frankvnk 2:035d551759a5 154 arm_cfft_f32(S, samples, 0, 1);
frankvnk 0:0c037aff5039 155 arm_cmplx_mag_f32(samples, magnitudes, FFT_SIZE);
frankvnk 0:0c037aff5039 156
Soto 3:6c9dabbb7261 157 for (int i = 0; i < FFT_SIZE/2+1; ++i) {
Soto 3:6c9dabbb7261 158 // pc.printf("%f, %f\r\n", i*freq, magnitudes[i]); //Esta línea se activa solo si se desea conocer la magnitudes generada por la FFT
Soto 3:6c9dabbb7261 159 //Sección de código que permite obtener el valor de frecuencia y magnitud mayor de los calculados
Soto 3:6c9dabbb7261 160 if (magnitudes[i]>max[1]){
Soto 3:6c9dabbb7261 161 max[0]=i*freq;
Soto 3:6c9dabbb7261 162 max[1]=magnitudes[i]; //Habilitar solo para debuggeo
Soto 3:6c9dabbb7261 163 }
Soto 3:6c9dabbb7261 164 }
Soto 3:6c9dabbb7261 165
Soto 3:6c9dabbb7261 166 //Dependiendo del valor de max[0], que representa la frecuencia natural, indicara la persona
Soto 3:6c9dabbb7261 167 //que esta chiflando y la silla ejecutara una rutina especifica
Soto 3:6c9dabbb7261 168 if(max[0]<=1600 && max[0]>=1350){
Soto 3:6c9dabbb7261 169
Soto 3:6c9dabbb7261 170 pc.printf("\n %f\n", max[0]);
Soto 3:6c9dabbb7261 171 pc.printf("\nChifla Jessi\n");
Soto 3:6c9dabbb7261 172 Derecha();
Soto 3:6c9dabbb7261 173 wait(1);
Soto 3:6c9dabbb7261 174 Retroceder();
Soto 3:6c9dabbb7261 175 wait(1);
Soto 3:6c9dabbb7261 176 Alto();
Soto 3:6c9dabbb7261 177 }
Soto 3:6c9dabbb7261 178 else if (max[0]<=3500 && max[0]>= 3000){
Soto 3:6c9dabbb7261 179
Soto 3:6c9dabbb7261 180 pc.printf("\n %f\n", max[0]);
Soto 3:6c9dabbb7261 181 pc.printf("\nChifla Soto\n");
Soto 3:6c9dabbb7261 182 Avanzar();
Soto 3:6c9dabbb7261 183 wait(1);
Soto 3:6c9dabbb7261 184 Izquierda();
Soto 3:6c9dabbb7261 185 wait(1);
Soto 3:6c9dabbb7261 186 Alto();
Soto 3:6c9dabbb7261 187 }
Soto 3:6c9dabbb7261 188 else if (max[0]<=2200 && max[0]>= 2500){
Soto 3:6c9dabbb7261 189
Soto 3:6c9dabbb7261 190 pc.printf("\n %f\n", max[0]);
Soto 3:6c9dabbb7261 191 pc.printf("\nChifla Snais\n");
frankvnk 0:0c037aff5039 192 }
frankvnk 0:0c037aff5039 193
Soto 3:6c9dabbb7261 194 //Se reinician las variables máximas
Soto 3:6c9dabbb7261 195 max[0]=0;
Soto 3:6c9dabbb7261 196 max[1]=0;
Soto 3:6c9dabbb7261 197
Soto 3:6c9dabbb7261 198 // Se vuelve a incertar la interrupción de muestreo
frankvnk 0:0c037aff5039 199 samplingBegin();
frankvnk 0:0c037aff5039 200 }
frankvnk 0:0c037aff5039 201 }
Soto 3:6c9dabbb7261 202 }