Revision 3:6c9dabbb7261, committed 2015-05-03

Comitter:

Soto

Date:

Sun May 03 22:53:37 2015 +0000

Parent:

2:035d551759a5

Commit message:

Proyecto de control de una silla de rueda mediante analisis FFT

Changed in this revision

diff -r 035d551759a5 -r 6c9dabbb7261 main.cpp
--- a/main.cpp	Sat Mar 08 19:30:20 2014 +0000
+++ b/main.cpp	Sun May 03 22:53:37 2015 +0000
@@ -1,338 +1,138 @@
-// Audio Spectrum Display
-// Copyright 2013 Tony DiCola (tony@tonydicola.com)
-// Code ported from the guide at http://learn.adafruit.com/fft-fun-with-fourier-transforms?view=all
-
-#include "mbed.h"
-#include "NVIC_set_all_priorities.h"
+#include "mbed.h" //Librería default de mbed
+#include "NVIC_set_all_priorities.h" //Librería para modificar todas las solicitudes de interrupciones al mismo tiempo
 #include <ctype.h>
-#include "arm_math.h"
+#include "arm_math.h" //Librería que contiene funciones de tranformada de fourier
 #include "arm_const_structs.h"
-#include "hsi2rgbw_pwm.h"
-#include "FastAnalogIn.h"
-
-Serial pc(USBTX, USBRX);
-
-FastAnalogIn   Audio(PTC2);
+#include "FastAnalogIn.h" //Librería modificada basada en la función de AnalogIn que reduce el tiempo de procesamiento de las señales ADC
 
-//#define RGBW_ext // Disable this line when you want to use the KL25Z on-board RGB LED.
+//**********Declaraciones devariables para la silla****************
 
-#ifndef RGBW_ext
-// HSI to RGB conversion with direct output to PWM channels - on-board RGB LED
-hsi2rgbw_pwm led(LED_RED, LED_GREEN, LED_BLUE);
-#else
-// HSI to RGBW conversion with direct output to external PWM channels - RGBW LED
-hsi2rgbw_pwm led(PTD4, PTA12, PTA4, PTA5); //Red, Green, Blue, White
-#endif
-
-// Dummy ISR for disabling NMI on PTA4 - !! DO NOT REMOVE THIS !!
-// More info at https://mbed.org/questions/1387/How-can-I-access-the-FTFA_FOPT-register-/
-extern "C" void NMI_Handler() {
-    DigitalIn test(PTA4);
-}
-
+DigitalOut EnableLeft(PTC9);//Salida que habilitara o deshabilitara el puente H de la llanta izquierda
+DigitalOut EnableRight(PTC8);//Salida que habilitara o deshabilitara el puente H de la llanta derecha
 
-////////////////////////////////////////////////////////////////////////////////
-// CONFIGURATION
-// These values can be changed to alter the behavior of the spectrum display.
-// KL25Z limitations
-// -----------------
-// - When used with the Spectrogram python script :
-//   There is a substantial time lag between the music and the screen output.
-//   Max allowed SAMPLE_RATE_HZ is 40000
-//   Max allowed FFT_SIZE is 64
-////////////////////////////////////////////////////////////////////////////////
-
-int SLOWDOWN = 4;                       // Create an optical delay in spectrumLoop - useful when only one RGB led is used.
-                                        // Only active when nonzero.
-                                        // A value >= 1000 and <= 1000 + PIXEL_COUNT fixes the output to a single frequency
-                                        // window = a single color.
-int SAMPLE_RATE_HZ = 40000;             // Sample rate of the audio in hertz.
-float SPECTRUM_MIN_DB = 30.0;           // Audio intensity (in decibels) that maps to low LED brightness.
-float SPECTRUM_MAX_DB = 80.0;           // Audio intensity (in decibels) that maps to high LED brightness.
-int LEDS_ENABLED = 1;                   // Control if the LED's should display the spectrum or not.  1 is true, 0 is false.
-                                        // Useful for turning the LED display on and off with commands from the serial port.
-const int FFT_SIZE = 64;                // Size of the FFT.
-const int PIXEL_COUNT = 32;             // Number of pixels.  You should be able to increase this without
-                                        // any other changes to the program.
-const int MAX_CHARS = 65;               // Max size of the input command buffer
+DigitalOut RightControl1(PTA5);//Salidas que controlan los estados del puente H de la llanta derecha
+DigitalOut RightControl2(PTA4);
 
-////////////////////////////////////////////////////////////////////////////////
-// INTERNAL STATE
-// These shouldn't be modified unless you know what you're doing.
-////////////////////////////////////////////////////////////////////////////////
-const static arm_cfft_instance_f32 *S;
-Ticker samplingTimer;
-float samples[FFT_SIZE*2];
-float magnitudes[FFT_SIZE];
-int sampleCounter = 0;
-char commandBuffer[MAX_CHARS];
-float frequencyWindow[PIXEL_COUNT+1];
-float hues[PIXEL_COUNT];
-bool commandRecv = 0;
-////////////////////////////////////////////////////////////////////////////////
-// UTILITY FUNCTIONS
-////////////////////////////////////////////////////////////////////////////////
+DigitalOut LeftControl1(PTA12);//Salidas que controlan los estados del puente H de la llanta izquierda
+DigitalOut LeftControl2(PTD4);
+//*******************************************************
 
-void rxisr() {
-    char c = pc.getc();
-    // Add any characters that aren't the end of a command (semicolon) to the input buffer.
-    if (c != ';') {
-        c = toupper(c);
-        strncat(commandBuffer, &c, 1);
-    } else {
-        // Parse the command because an end of command token was encountered.
-        commandRecv = 1;
-    }
+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)
+FastAnalogIn   segnal(PTC2); //Se declara el pin que recibirá la señal análoga del sensor
+
+extern "C" void NMI_Handler() {
 }
 
-// Compute the average magnitude of a target frequency window vs. all other frequencies.
-void windowMean(float* magnitudes, int lowBin, int highBin, float* windowMean, float* otherMean)
-{
-    *windowMean = 0;
-    *otherMean = 0;
-    // Notice the first magnitude bin is skipped because it represents the
-    // average power of the signal.
-    for (int i = 1; i < FFT_SIZE/2; ++i) {
-        if (i >= lowBin && i <= highBin) {
-            *windowMean += magnitudes[i];
-        } else {
-            *otherMean += magnitudes[i];
-        }
-    }
-    *windowMean /= (highBin - lowBin) + 1;
-    *otherMean /= (FFT_SIZE / 2 - (highBin - lowBin));
-}
+////////////////////////////////////////////////////////////////////////////////
+// CONFIGURACION
+//Estos valores puedne modificarse para modificar los parametros de la transformada
+////////////////////////////////////////////////////////////////////////////////
 
-// Convert a frequency to the appropriate FFT bin it will fall within.
-int frequencyToBin(float frequency)
-{
-    float binFrequency = float(SAMPLE_RATE_HZ) / float(FFT_SIZE);
-    return int(frequency / binFrequency);
-}
-
+int SAMPLE_RATE_HZ = 40000;       // Frecuencian de muestreo en HZ del sistema
+const int FFT_SIZE = 1024;           // Número de valores para la transformada rápida
+float freq = 40000.0/1024.0;           // Frecuencia de activación de la interrupción de muestreo
+float max[2];                        // Arreglo que almacena la frecuencia y magnitud mayores del espectro de Fourier
+char blue_freq=0;                    // Variable que almacena el valor que se envia por bluetooth de los BPM
 
 ////////////////////////////////////////////////////////////////////////////////
-// SPECTRUM DISPLAY FUNCTIONS
-///////////////////////////////////////////////////////////////////////////////
-
-void spectrumSetup()
-{
-    // Set the frequency window values by evenly dividing the possible frequency
-    // spectrum across the number of neo pixels.
-    float windowSize = (SAMPLE_RATE_HZ / 2.0) / float(PIXEL_COUNT);
-    for (int i = 0; i < PIXEL_COUNT+1; ++i) {
-        frequencyWindow[i] = i*windowSize;
-    }
-    // Evenly spread hues across all pixels.
-    for (int i = 0; i < PIXEL_COUNT; ++i) {
-        hues[i] = 360.0*(float(i)/float(PIXEL_COUNT-1));
-    }
-}
+// ESTADO INTERNO
+// Configuraciones necesarias para el correcto funcionaiento del programa
+////////////////////////////////////////////////////////////////////////////////
 
-void spectrumLoop()
-{
-    // Update each LED based on the intensity of the audio
-    // in the associated frequency window.
-    static int SLrpt = 0, SLpixcnt = 0;
-    int SLpixend = 0;
-    float intensity, otherMean;
-    if(SLOWDOWN != 0)
-    {
-        if(SLOWDOWN >= 1000)
-        {
-            if(SLOWDOWN <= (1000 + PIXEL_COUNT-1))
-            {
-                SLpixcnt = SLOWDOWN - 1000;
-                SLrpt = 0;
-                SLpixend = SLpixcnt + 1;
-            }
-            else
-                SLOWDOWN = 0;
-        }
-        else
-        {
-            SLrpt++;
-            if (SLrpt >= SLOWDOWN)
-            {
-                SLrpt = 0;
-                SLpixcnt = SLpixcnt < PIXEL_COUNT-1 ? ++SLpixcnt : 0;
-            }
-            SLpixend = SLpixcnt + 1;
-        }
-    }
-    else
-    {
-        SLpixcnt = 0;
-        SLrpt = 0;
-        SLpixend = PIXEL_COUNT;
-    }
-    for (int i = SLpixcnt; i < SLpixend; ++i) {
-        windowMean(magnitudes,
-                   frequencyToBin(frequencyWindow[i]),
-                   frequencyToBin(frequencyWindow[i+1]),
-                   &intensity,
-                   &otherMean);
-        // Convert intensity to decibels.
-        intensity = 20.0*log10(intensity);
-        // Scale the intensity and clamp between 0 and 1.0.
-        intensity -= SPECTRUM_MIN_DB;
-        intensity = intensity < 0.0 ? 0.0 : intensity;
-        intensity /= (SPECTRUM_MAX_DB-SPECTRUM_MIN_DB);
-        intensity = intensity > 1.0 ? 1.0 : intensity;
-        led.hsi2rgbw(hues[i], 1.0, intensity);
-    }
-}
-
+const static arm_cfft_instance_f32 *S;
+Ticker samplingTimer;                       //objeto creado para habilitar las interrupciones con lso métodos de ticker
+float samples[FFT_SIZE*2];                  //Arreglo en el que se almacenan las muestras del tomadas ADC
+float magnitudes[FFT_SIZE];                 //Arreglo donde se almacenan las magnitudes de la FFT
+int sampleCounter = 0;                      //Contador del número de muestras tomadas
 
 ////////////////////////////////////////////////////////////////////////////////
-// SAMPLING FUNCTIONS
+// FUNCIONES DE MUESTREO
 ////////////////////////////////////////////////////////////////////////////////
 
+//Esta función permite realizar el muestreo de datos, se realiza como interrupción para asegurar el tiempo de muestreo deseado
 void samplingCallback()
 {
-    // Read from the ADC and store the sample data
-    samples[sampleCounter] = (1023 * Audio) - 511.0f;
-    // Complex FFT functions require a coefficient for the imaginary part of the input.
-    // Since we only have real data, set this coefficient to zero.
+    // Lectura del ADC y almacenamiento del dato
+    samples[sampleCounter] = (1023 * segnal) - 511.0f; //Se ajusta el valor de un rango de 0-1 a 0-511
+    // La función que calcula la transformada requiere de un valor imaginario, en este caso se le asigna 0 
+    // ya que los valores muestreados son solamente reales.
     samples[sampleCounter+1] = 0.0;
-    // Update sample buffer position and stop after the buffer is filled
+    // Se ajusta la posición en el arreglo para almacenar el siguiente valor real
     sampleCounter += 2;
+    //En caso de que el valor de sample counter sobrepase el tamaño del arreglo de almacenamiento se retira la interrupción
+    //de muestreo del programa
     if (sampleCounter >= FFT_SIZE*2) {
         samplingTimer.detach();
     }
 }
 
+//Esta función permite reiniciar el contador de muestras e insertar nuevamente la interrupción de muestreo
 void samplingBegin()
 {
-    // Reset sample buffer position and start callback at necessary rate.
-    sampleCounter = 0;
-    samplingTimer.attach_us(&samplingCallback, 1000000/SAMPLE_RATE_HZ);
+    sampleCounter = 0; //Se reinicia el contador de muestras
+    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
 }
 
+//Función booleana que funciona como bandera para indicar que el meustreo de datos ha sido finalizado
 bool samplingIsDone()
 {
     return sampleCounter >= FFT_SIZE*2;
 }
 
+//************Funciones del funcionamiento de la silla
+void Avanzar(void){//Esta funcion tiene las salidas programadas para que la silla avance hace delante
+    RightControl1=0;
+    RightControl2=1;
+    LeftControl1=0;
+    LeftControl2=1;
+    EnableLeft=1;
+    EnableRight=1;
+    }
+    
+void Retroceder(void){//Esta funcion las salidas programadas para que la silla retroceda
+    RightControl1=1;
+    RightControl2=0;
+    LeftControl1=1;
+    LeftControl2=0;
+    EnableLeft=1;
+    EnableRight=1;
+    }
+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)
+    LeftControl1=0;
+    LeftControl2=1;
+    EnableLeft=1;
+    EnableRight=0;
+    }
+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)
+    RightControl1=0;
+    RightControl2=1;
+    EnableLeft=0;
+    EnableRight=1;
+    }
+void Alto(void){//Esta fucnion apaga los enables del puente H y por lo tanto la silla para
+    EnableLeft=0;
+    EnableRight=0;
+    }
+//************************************************
 
 ////////////////////////////////////////////////////////////////////////////////
-// COMMAND PARSING FUNCTIONS
-// These functions allow parsing simple commands input on the serial port.
-// Commands allow reading and writing variables that control the device.
-//
-// All commands must end with a semicolon character.
-//
-// Example commands are:
-// GET SAMPLE_RATE_HZ;
-// - Get the sample rate of the device.
-// SET SAMPLE_RATE_HZ 400;
-// - Set the sample rate of the device to 400 hertz.
-//
-////////////////////////////////////////////////////////////////////////////////
-
-void parseCommand(char* command)
-{
-    if (strcmp(command, "GET MAGNITUDES") == 0) {
-        for (int i = 0; i < FFT_SIZE; ++i) {
-            printf("%f\r\n", magnitudes[i]);
-        }
-    } else if (strcmp(command, "GET SAMPLES") == 0) {
-        for (int i = 0; i < FFT_SIZE*2; i+=2) {
-            printf("%f\r\n", samples[i]);
-        }
-    } else if (strcmp(command, "GET FFT_SIZE") == 0) {
-        printf("%d\r\n", FFT_SIZE);
-    } else if (strcmp(command, "GET SAMPLE_RATE_HZ") == 0) {
-        printf("%d\r\n", SAMPLE_RATE_HZ);
-    } else if (strstr(command, "SET SAMPLE_RATE_HZ") != NULL) {
-        SAMPLE_RATE_HZ = (typeof(SAMPLE_RATE_HZ)) atof(command+(sizeof("SET SAMPLE_RATE_HZ")-1));
-    } else if (strcmp(command, "GET LEDS_ENABLED") == 0) {
-        printf("%d\r\n", LEDS_ENABLED);
-    } else if (strstr(command, "SET LEDS_ENABLED") != NULL) {
-        LEDS_ENABLED = (typeof(LEDS_ENABLED)) atof(command+(sizeof("SET LEDS_ENABLED")-1));
-    } else if (strcmp(command, "GET SPECTRUM_MIN_DB") == 0) {
-        printf("%f\r\n", SPECTRUM_MIN_DB);
-    } else if (strstr(command, "SET SPECTRUM_MIN_DB") != NULL) {
-        SPECTRUM_MIN_DB = (typeof(SPECTRUM_MIN_DB)) atof(command+(sizeof("SET SPECTRUM_MIN_DB")-1));
-    } else if (strcmp(command, "GET SPECTRUM_MAX_DB") == 0) {
-        printf("%f\r\n", SPECTRUM_MAX_DB);
-    } else if (strstr(command, "SET SPECTRUM_MAX_DB") != NULL) {
-        SPECTRUM_MAX_DB = (typeof(SPECTRUM_MAX_DB)) atof(command+(sizeof("SET SPECTRUM_MAX_DB")-1));
-    } else if (strcmp(command, "GET SLOWDOWN") == 0) {
-        printf("%d\r\n", SLOWDOWN);
-    } else if (strstr(command, "SET SLOWDOWN") != NULL) {
-        SLOWDOWN = (typeof(SLOWDOWN)) atoi(command+(sizeof("SET SLOWDOWN")-1));
-    }
-
-    // Update spectrum display values if sample rate was changed.
-    if (strstr(command, "SET SAMPLE_RATE_HZ ") != NULL) {
-        spectrumSetup();
-    }
-
-    // Turn off the LEDs if the state changed.
-    if (LEDS_ENABLED == 0) {
-    }
-}
-
-void parserLoop()
-{
-    // Process any incoming characters from the serial port
-    while (pc.readable()) {
-        char c = pc.getc();
-        // Add any characters that aren't the end of a command (semicolon) to the input buffer.
-        if (c != ';') {
-            c = toupper(c);
-            strncat(commandBuffer, &c, 1);
-        } else {
-            // Parse the command because an end of command token was encountered.
-            parseCommand(commandBuffer);
-            // Clear the input buffer
-            memset(commandBuffer, 0, sizeof(commandBuffer));
-        }
-    }
-}
-
-////////////////////////////////////////////////////////////////////////////////
-// MAIN FUNCTION
+// MAIN DEL PROGRAMA
 ////////////////////////////////////////////////////////////////////////////////
 
 int main()
 {
+    //Configuración de las solicitudes de interrupción
     NVIC_set_all_irq_priorities(1);
-    NVIC_SetPriority(UART0_IRQn, 0);
-    // Set up serial port.
-    pc.baud (38400);
-    pc.attach(&rxisr);
-#ifndef RGBW_ext
-    led.invertpwm(1); //On-board KL25Z RGB LED uses common anode.
-#endif 
-    // Clear the input command buffer
-    memset(commandBuffer, 0, sizeof(commandBuffer));
+    
+    //Configuración de la velocidad de la comunicación serial
+    pc.baud (38400); //Velocidad de la comunicación USB
 
-    // Initialize spectrum display
-    spectrumSetup();
-
-    // Begin sampling audio
+    // Se incerta la interrupción de muestreo del ADC
     samplingBegin();
 
-    // Init arm_ccft_32
+    // Init arm_ccft_32 el registro cambiara dependiendo de la variable FFT_SIZE
     switch (FFT_SIZE)
     {
-    case 16:
-        S = & arm_cfft_sR_f32_len16;
-        break;
-    case 32:
-        S = & arm_cfft_sR_f32_len32;
-        break;
-    case 64:
-        S = & arm_cfft_sR_f32_len64;
-        break;
-    case 128:
-        S = & arm_cfft_sR_f32_len128;
-        break;
-    case 256:
-        S = & arm_cfft_sR_f32_len256;
-        break;
     case 512:
         S = & arm_cfft_sR_f32_len512;
         break;
@@ -348,30 +148,55 @@
     }
 
     while(1) {
-        // Calculate FFT if a full sample is available.
+        // Se calcula la FFT si se ha terminado el muestreo
         if (samplingIsDone()) {
-            // Run FFT on sample data.
-            // Run FFT on sample data.
+            
             arm_cfft_f32(S, samples, 0, 1);
-            // Calculate magnitude of complex numbers output by the FFT.
             arm_cmplx_mag_f32(samples, magnitudes, FFT_SIZE);
 
-            if (LEDS_ENABLED == 1) {
-                spectrumLoop();
+            for (int i = 0; i < FFT_SIZE/2+1; ++i) {
+            //    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
+                //Sección de código que permite obtener el valor de frecuencia y magnitud mayor de los calculados
+                if (magnitudes[i]>max[1]){
+                        max[0]=i*freq;
+                        max[1]=magnitudes[i]; //Habilitar solo para debuggeo
+                }
+            }
+             
+            //Dependiendo del valor de max[0], que representa la frecuencia natural, indicara la persona
+            //que esta chiflando y la silla ejecutara una rutina especifica
+            if(max[0]<=1600 && max[0]>=1350){
+            
+                pc.printf("\n %f\n", max[0]); 
+                pc.printf("\nChifla Jessi\n");
+                Derecha();
+                wait(1);
+                Retroceder();
+                wait(1);
+                Alto();
+            } 
+            else if (max[0]<=3500 && max[0]>= 3000){
+                
+                pc.printf("\n %f\n", max[0]); 
+                pc.printf("\nChifla Soto\n");
+                Avanzar();
+                wait(1);
+                Izquierda();
+                wait(1);
+                Alto();
+            } 
+            else if (max[0]<=2200 && max[0]>= 2500){
+                   
+                   pc.printf("\n %f\n", max[0]); 
+                pc.printf("\nChifla Snais\n");
             }
 
-            // Restart audio sampling.
+            //Se reinician las variables máximas
+            max[0]=0;
+            max[1]=0;
+            
+            // Se vuelve a incertar la interrupción de muestreo
             samplingBegin();
         }
-
-        // Parse any pending commands.
-        if(commandRecv) {
-//            pc.attach(NULL);
-            parseCommand(commandBuffer);
-            commandRecv = 0;
-            // Clear the input buffer
-            memset(commandBuffer, 0, sizeof(commandBuffer));
-//            pc.attach(&rxisr);
-        }
     }
-}
+}
\ No newline at end of file