main.cpp

00001 #include "mbed.h" //Librería default de mbed
00002 #include "NVIC_set_all_priorities.h" //Librería para modificar todas las solicitudes de interrupciones al mismo tiempo
00003 #include <ctype.h>
00004 #include "arm_math.h" //Librería que contiene funciones de tranformada de fourier
00005 #include "arm_const_structs.h"
00006 #include "FastAnalogIn.h" //Librería modificada basada en la función de AnalogIn que reduce el tiempo de procesamiento de las señales ADC
00007 
00008 //**********Declaraciones devariables para la silla****************
00009 
00010 DigitalOut EnableLeft(PTC9);//Salida que habilitara o deshabilitara el puente H de la llanta izquierda
00011 DigitalOut EnableRight(PTC8);//Salida que habilitara o deshabilitara el puente H de la llanta derecha
00012 
00013 DigitalOut RightControl1(PTA5);//Salidas que controlan los estados del puente H de la llanta derecha
00014 DigitalOut RightControl2(PTA4);
00015 
00016 DigitalOut LeftControl1(PTA12);//Salidas que controlan los estados del puente H de la llanta izquierda
00017 DigitalOut LeftControl2(PTD4);
00018 //*******************************************************
00019 
00020 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)
00021 FastAnalogIn   segnal(PTC2); //Se declara el pin que recibirá la señal análoga del sensor
00022 
00023 extern "C" void NMI_Handler() {
00024 }
00025 
00026 ////////////////////////////////////////////////////////////////////////////////
00027 // CONFIGURACION
00028 //Estos valores puedne modificarse para modificar los parametros de la transformada
00029 ////////////////////////////////////////////////////////////////////////////////
00030 
00031 int SAMPLE_RATE_HZ = 40000;       // Frecuencian de muestreo en HZ del sistema
00032 const int FFT_SIZE = 1024;           // Número de valores para la transformada rápida
00033 float freq = 40000.0/1024.0;           // Frecuencia de activación de la interrupción de muestreo
00034 float max[2];                        // Arreglo que almacena la frecuencia y magnitud mayores del espectro de Fourier
00035 char blue_freq=0;                    // Variable que almacena el valor que se envia por bluetooth de los BPM
00036 
00037 ////////////////////////////////////////////////////////////////////////////////
00038 // ESTADO INTERNO
00039 // Configuraciones necesarias para el correcto funcionaiento del programa
00040 ////////////////////////////////////////////////////////////////////////////////
00041 
00042 const static arm_cfft_instance_f32 *S;
00043 Ticker samplingTimer;                       //objeto creado para habilitar las interrupciones con lso métodos de ticker
00044 float samples[FFT_SIZE*2];                  //Arreglo en el que se almacenan las muestras del tomadas ADC
00045 float magnitudes[FFT_SIZE];                 //Arreglo donde se almacenan las magnitudes de la FFT
00046 int sampleCounter = 0;                      //Contador del número de muestras tomadas
00047 
00048 ////////////////////////////////////////////////////////////////////////////////
00049 // FUNCIONES DE MUESTREO
00050 ////////////////////////////////////////////////////////////////////////////////
00051 
00052 //Esta función permite realizar el muestreo de datos, se realiza como interrupción para asegurar el tiempo de muestreo deseado
00053 void samplingCallback()
00054 {
00055     // Lectura del ADC y almacenamiento del dato
00056     samples[sampleCounter] = (1023 * segnal) - 511.0f; //Se ajusta el valor de un rango de 0-1 a 0-511
00057     // La función que calcula la transformada requiere de un valor imaginario, en este caso se le asigna 0 
00058     // ya que los valores muestreados son solamente reales.
00059     samples[sampleCounter+1] = 0.0;
00060     // Se ajusta la posición en el arreglo para almacenar el siguiente valor real
00061     sampleCounter += 2;
00062     //En caso de que el valor de sample counter sobrepase el tamaño del arreglo de almacenamiento se retira la interrupción
00063     //de muestreo del programa
00064     if (sampleCounter >= FFT_SIZE*2) {
00065         samplingTimer.detach();
00066     }
00067 }
00068 
00069 //Esta función permite reiniciar el contador de muestras e insertar nuevamente la interrupción de muestreo
00070 void samplingBegin()
00071 {
00072     sampleCounter = 0; //Se reinicia el contador de muestras
00073     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
00074 }
00075 
00076 //Función booleana que funciona como bandera para indicar que el meustreo de datos ha sido finalizado
00077 bool samplingIsDone()
00078 {
00079     return sampleCounter >= FFT_SIZE*2;
00080 }
00081 
00082 //************Funciones del funcionamiento de la silla
00083 void Avanzar(void){//Esta funcion tiene las salidas programadas para que la silla avance hace delante
00084     RightControl1=0;
00085     RightControl2=1;
00086     LeftControl1=0;
00087     LeftControl2=1;
00088     EnableLeft=1;
00089     EnableRight=1;
00090     }
00091     
00092 void Retroceder(void){//Esta funcion las salidas programadas para que la silla retroceda
00093     RightControl1=1;
00094     RightControl2=0;
00095     LeftControl1=1;
00096     LeftControl2=0;
00097     EnableLeft=1;
00098     EnableRight=1;
00099     }
00100 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)
00101     LeftControl1=0;
00102     LeftControl2=1;
00103     EnableLeft=1;
00104     EnableRight=0;
00105     }
00106 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)
00107     RightControl1=0;
00108     RightControl2=1;
00109     EnableLeft=0;
00110     EnableRight=1;
00111     }
00112 void Alto(void){//Esta fucnion apaga los enables del puente H y por lo tanto la silla para
00113     EnableLeft=0;
00114     EnableRight=0;
00115     }
00116 //************************************************
00117 
00118 ////////////////////////////////////////////////////////////////////////////////
00119 // MAIN DEL PROGRAMA
00120 ////////////////////////////////////////////////////////////////////////////////
00121 
00122 int main()
00123 {
00124     //Configuración de las solicitudes de interrupción
00125     NVIC_set_all_irq_priorities(1);
00126     
00127     //Configuración de la velocidad de la comunicación serial
00128     pc.baud (38400); //Velocidad de la comunicación USB
00129 
00130     // Se incerta la interrupción de muestreo del ADC
00131     samplingBegin();
00132 
00133     // Init arm_ccft_32 el registro cambiara dependiendo de la variable FFT_SIZE
00134     switch (FFT_SIZE)
00135     {
00136     case 512:
00137         S = & arm_cfft_sR_f32_len512;
00138         break;
00139     case 1024:
00140         S = & arm_cfft_sR_f32_len1024;
00141         break;
00142     case 2048:
00143         S = & arm_cfft_sR_f32_len2048;
00144         break;
00145     case 4096:
00146         S = & arm_cfft_sR_f32_len4096;
00147         break;
00148     }
00149 
00150     while(1) {
00151         // Se calcula la FFT si se ha terminado el muestreo
00152         if (samplingIsDone()) {
00153             
00154             arm_cfft_f32(S, samples, 0, 1);
00155             arm_cmplx_mag_f32(samples, magnitudes, FFT_SIZE);
00156 
00157             for (int i = 0; i < FFT_SIZE/2+1; ++i) {
00158             //    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
00159                 //Sección de código que permite obtener el valor de frecuencia y magnitud mayor de los calculados
00160                 if (magnitudes[i]>max[1]){
00161                         max[0]=i*freq;
00162                         max[1]=magnitudes[i]; //Habilitar solo para debuggeo
00163                 }
00164             }
00165              
00166             //Dependiendo del valor de max[0], que representa la frecuencia natural, indicara la persona
00167             //que esta chiflando y la silla ejecutara una rutina especifica
00168             if(max[0]<=1600 && max[0]>=1350){
00169             
00170                 pc.printf("\n %f\n", max[0]); 
00171                 pc.printf("\nChifla Jessi\n");
00172                 Derecha();
00173                 wait(1);
00174                 Retroceder();
00175                 wait(1);
00176                 Alto();
00177             } 
00178             else if (max[0]<=3500 && max[0]>= 3000){
00179                 
00180                 pc.printf("\n %f\n", max[0]); 
00181                 pc.printf("\nChifla Soto\n");
00182                 Avanzar();
00183                 wait(1);
00184                 Izquierda();
00185                 wait(1);
00186                 Alto();
00187             } 
00188             else if (max[0]<=2200 && max[0]>= 2500){
00189                    
00190                    pc.printf("\n %f\n", max[0]); 
00191                 pc.printf("\nChifla Snais\n");
00192             }
00193 
00194             //Se reinician las variables máximas
00195             max[0]=0;
00196             max[1]=0;
00197             
00198             // Se vuelve a incertar la interrupción de muestreo
00199             samplingBegin();
00200         }
00201     }
00202 }