FFT_BUENA

Dependencies:   FastAnalogIn HSI2RGBW_PWM NVIC_set_all_priorities mbed-dsp mbed

Fork of KL25Z_FFT_Demo by Frank Vannieuwkerke

main.cpp

Committer:
oscarmtzman
Date:
2015-04-30
Revision:
8:391e30708e7c
Parent:
7:18e9a56bef9a

File content as of revision 8:391e30708e7c:

#include "mbed.h"
#include "NVIC_set_all_priorities.h"
#include <ctype.h>
#include "arm_math.h"
#include "arm_const_structs.h"
#include "hsi2rgbw_pwm.h"
#include "FastAnalogIn.h"

DigitalOut led1(PTA2);
DigitalOut led2(PTC0);
DigitalOut led3(PTC3);
DigitalOut led4(PTC4);
DigitalOut led5(PTC5);
DigitalOut led6(PTC6);
DigitalOut led7(PTC10);
DigitalOut led8(PTC11);
AnalogIn pot(A0);

float pulso=0;
int fmin=50;
int fmax=120;
float sum=(fmax-fmin)/8;

Serial pc(USBTX, USBRX);

float pulsocalculado=0;

float qrs2[512]= {0};
float fs=102.4;
float ts=1/fs;
int tiempo=5;
float pi=3.1416;
float l=0;
float b=0;
float a=1.6;
int n=100;
float qrs1=0;


#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);
}


////////////////////////////////////////////////////////////////////////////////
// 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 = 512;                // 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

////////////////////////////////////////////////////////////////////////////////
// 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
////////////////////////////////////////////////////////////////////////////////

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;
    }
}

// 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));
}

// 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);
}


////////////////////////////////////////////////////////////////////////////////
// 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));
    }
}

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);                                                          //Modificar
        // 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);
    }
}


////////////////////////////////////////////////////////////////////////////////
// SAMPLING FUNCTIONS
////////////////////////////////////////////////////////////////////////////////

void samplingCallback()
{
    // Read from the ADC and store the sample data
    samples[sampleCounter] = qrs1+qrs2[(sampleCounter+1)/2];
    // Complex FFT functions require a coefficient for the imaginary part of the input.
    // Since we only have real data, set this coefficient to zero.
    samples[sampleCounter+1] = 0.0;
    // Update sample buffer position and stop after the buffer is filled
    sampleCounter += 2;
    if (sampleCounter >= FFT_SIZE*2) {
        samplingTimer.detach();
    }
}

void samplingBegin()
{
    // Reset sample buffer position and start callback at necessary rate.
    sampleCounter = 0;
    samplingTimer.attach_us(&samplingCallback, 1000000/SAMPLE_RATE_HZ);
}

bool samplingIsDone()
{
    return sampleCounter >= FFT_SIZE*2;
}


////////////////////////////////////////////////////////////////////////////////
// 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
////////////////////////////////////////////////////////////////////////////////

int main()
{
    NVIC_set_all_irq_priorities(1);
    NVIC_SetPriority(UART0_IRQn, 0);
    // Set up serial port.
    pc.baud (9600);
    pc.attach(&rxisr);

    float harm[512];
    int inmax=0,max=0;

    pulso=fmin+(fmax-fmin)*pot.read();
    pc.printf("%f\n\n\n",pulso);
    l=30/pulso;
    b=(2*l)/0.11;
    qrs1=(a/(2*b))*(2-b);



    for(int i=1; i<=n; i=i+1) {
        for(int j=0; j<512; j=j+1) {
            harm[j]=(((2*b*a)/(i*i*pi*pi))*(1-cos((i*pi)/b)))*cos((i*pi*(j+1)*ts)/l);
        }
        for(int k=0; k<512; k=k+1) {
            qrs2[k]=qrs2[k]+harm[k];
        }
    }







#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));

    // Initialize spectrum display
    spectrumSetup();

    // Begin sampling audio
    samplingBegin();

    // Init arm_ccft_32
    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;
        case 1024:
            S = & arm_cfft_sR_f32_len1024;
            break;
        case 2048:
            S = & arm_cfft_sR_f32_len2048;
            break;
        case 4096:
            S = & arm_cfft_sR_f32_len4096;
            break;
    }

    while(1) {
        // Calculate FFT if a full sample is available.
        
        
        if(abs(fmin+(fmax-fmin)*pot.read()-pulso)>.5) {
                wait(2);
                pulso=fmin+(fmax-fmin)*pot.read();
                pc.printf("\n\n\n%f\n\n\n",pulso);
                l=30/pulso;
                b=(2*l)/0.11;
                qrs1=(a/(2*b))*(2-b);
                
                for(int i=1; i<=n; i=i+1) {
                    for(int j=0; j<512; j=j+1) {
                        harm[j]=(((2*b*a)/(i*i*pi*pi))*(1-cos((i*pi)/b)))*cos((i*pi*(j+1)*ts)/l);
                    }
                    for(int k=0; k<512; k=k+1) {
                        qrs2[k]=qrs2[k]+harm[k];
                    }
                }
                // Begin sampling audio
                samplingBegin();
            }
        
        
        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);


            
            for(int i=1; i<FFT_SIZE/2; i=i+1) {
                if(magnitudes[i]>max&&abs(fs/(FFT_SIZE-1)*i*60-pulso)<8) {
                    max=magnitudes[i];
                    inmax=i;
                }
            }

            pulsocalculado=fs/(FFT_SIZE-1)*inmax*60;
            pc.printf("%f\n",pulsocalculado);

            if (pulsocalculado<(fmin+sum)) {
                led1=1;
                led2=0;
                led3=0;
                led4=0;
                led5=0;
                led6=0;
                led7=0;
                led8=0;
            }
            if (pulsocalculado>=(fmin+sum)&&pulsocalculado<(fmin+2*sum)) {
                led1=1;
                led2=1;
                led3=0;
                led4=0;
                led5=0;
                led6=0;
                led7=0;
                led8=0;
            }
            if (pulsocalculado>=(fmin+2*sum)&&pulsocalculado<(fmin+3*sum)) {
                led1=1;
                led2=1;
                led3=1;
                led4=0;
                led5=0;
                led6=0;
                led7=0;
                led8=0;
            }
            if (pulsocalculado>=(fmin+3*sum)&&pulsocalculado<(fmin+4*sum)) {
                led1=1;
                led2=1;
                led3=1;
                led4=1;
                led5=0;
                led6=0;
                led7=0;
                led8=0;
            }
            if (pulsocalculado>=(fmin+4*sum)&&pulsocalculado<(fmin+5*sum)) {
                led1=1;
                led2=1;
                led3=1;
                led4=1;
                led5=1;
                led6=0;
                led7=0;
                led8=0;
            }
            if (pulsocalculado>=(fmin+5*sum)&&pulsocalculado<(fmin+6*sum)) {
                led1=1;
                led2=1;
                led3=1;
                led4=1;
                led5=1;
                led6=1;
                led7=0;
                led8=0;
            }
            if (pulsocalculado>=(fmin+6*sum)&&pulsocalculado<(fmin+7*sum)) {
                led1=1;
                led2=1;
                led3=1;
                led4=1;
                led5=1;
                led6=1;
                led7=1;
                led8=0;
            }
            if (pulsocalculado>=(fmin+7*sum)) {
                led1=1;
                led2=1;
                led3=1;
                led4=1;
                led5=1;
                led6=1;
                led7=1;
                led8=1;
            }

            inmax=0;
            max=0;

            if (LEDS_ENABLED == 1) {
                spectrumLoop();
            }

            // Restart audio sampling.
            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);
        }
    }
}