FFT and nRF24 transmission / LaserGame 1A

Dependencies:   mbed nRF24L01P mbed-dsp

main.cpp

Committer:
villemejane
Date:
23 months ago
Revision:
3:627354c99ba0
Parent:
1:47d90ce030a3

File content as of revision 3:627354c99ba0:

/****************************************************************************/
/*  FFT d'un signal   - Nucleo                                              */
/****************************************************************************/
/*  LEnsE / Julien VILLEMEJANE       /   Institut d'Optique Graduate School */
/****************************************************************************/
/*  Brochage                                                                */
/*      TO COMPLETE                                                         */
/****************************************************************************/
/*  Test réalisé sur Nucléo-L476RG                                          */
/****************************************************************************/

#include "mbed.h"
#include "arm_math.h"
#include "dsp.h"
#include "arm_common_tables.h"
#include "arm_const_structs.h"
#include "nRF24L01P.h"

#define TRANSFER_SIZE           8
#define SAMPLES                 512             /* 256 real party and 256 imaginary parts */
#define FFT_SIZE                SAMPLES / 2     /* FFT size is always the same size as we have samples, so 256 in our case */
 
float32_t Input[SAMPLES];
float32_t Output[FFT_SIZE];
bool      trig=0;
int       indice = 0;

DigitalOut  myled(D13);
AnalogIn    myADC(A0);
Serial      debug_pc(USBTX, USBRX);
Ticker      timer;

// nRF24
char k;
char    dataToSend[TRANSFER_SIZE] = {0xAA, 0x01, 0x10, 0xF0,0xAA, 0x01, 0x10, 0xF0};
char    dataReceived[TRANSFER_SIZE] = {0};
char    rxDataCnt;

nRF24L01P       nRF24_mod(D11, D12, D13, D10, D9, PB_8);
// MOSI, MISO, SCK, CSN, CE, IRQ

#define largeur_signal_carre_equipe1      4300 /* a  corriger */
#define largeur_signal_carre_equipe2      40  /* a corriger */
#define epsilon  200

/* definition des entrees des 3 capteurs: epaule droit, gauche et ventre */
AnalogIn cap_ventre(A0);
AnalogIn cap_epaule_droit(A1);
AnalogIn cap_epaule_gauche(A2);  

float32_t Input_ventre[SAMPLES];
float32_t Output_ventre[FFT_SIZE];

float32_t Input_epaule_droit[SAMPLES];
float32_t Output_epaule_droit[FFT_SIZE];

float32_t Input_epaule_gauche[SAMPLES];
float32_t Output_epaule_gauche[FFT_SIZE];

int nbr_pts_equipe1;
int nbr_pts_equipe2;    

// Fonction d'initialisation du module BT nRF24L01
void initNRF24(){
    nRF24_mod.powerUp();
    wait_us(100000);
    nRF24_mod.setAirDataRate(NRF24L01P_DATARATE_250_KBPS);
    nRF24_mod.setRfFrequency(2400);
    wait_us(100000);
    debug_pc.printf( "nRF24L01+ Frequency    : %d MHz\r\n",  nRF24_mod.getRfFrequency() );
    debug_pc.printf( "nRF24L01+ Output power : %d dBm\r\n",  nRF24_mod.getRfOutputPower() );
    debug_pc.printf( "nRF24L01+ Data Rate    : %d kbps\r\n", nRF24_mod.getAirDataRate() );
    debug_pc.printf( "Transfers are grouped into %d characters\r\n", TRANSFER_SIZE );
    nRF24_mod.setTransferSize( TRANSFER_SIZE );
    nRF24_mod.setReceiveMode();
    nRF24_mod.enable();
}

// Fonction de test du module BT nRF24L01
void testNRF24(void){
    /* Lecture donnée depuis nRF24 */
    if ( nRF24_mod.readable() ) {

        // ...read the data into the receive buffer
        rxDataCnt = nRF24_mod.read( NRF24L01P_PIPE_P0, dataReceived, TRANSFER_SIZE);

        // Display the receive buffer contents via the host serial link
        debug_pc.printf("\tD = ");
        for ( int i = 0; i < rxDataCnt; i++ ) {
            debug_pc.printf(" %x \t", dataReceived[i]);
        }
        debug_pc.printf("\r\n");
    }   
    /* Transmission donnée depuis nRF24 */
    if(dataReceived[0] == 0){
        nRF24_mod.setRfFrequency(2400);
        nRF24_mod.write( NRF24L01P_PIPE_P0, dataToSend, TRANSFER_SIZE );
        debug_pc.printf( "SENDED\r\n");
    }
}

// echantillonnage
void sample(){
    myled = 1;
    if(indice < SAMPLES){
        Input_ventre[indice] = cap_ventre.read() - 0.5f;
        Input_epaule_droit[indice] = cap_ventre.read() - 0.5f;
        Input_epaule_droit[indice] = cap_ventre.read() - 0.5f;    
        
        //Real part NB removing DC offset
        Input_ventre[indice + 1] = 0;    
        Input_epaule_droit[indice + 1] = 0;
        Input_epaule_gauche[indice + 1] = 0;    
                 
        //Imaginary Part set to zero
        indice += 2;
    }
    else{ trig = 0; }
    myled = 0;
}
 
int main() {
    float maxValue_ventre;            // Max FFT value is stored here (la largeur du caré envoyé par la led) 
    float maxValue_epaule_droit;   
    float maxValue_epaule_gauche;  
    
    uint32_t maxIndex_ventre;         // Index in Output array where max value is
    uint32_t maxIndex_epaule_droit;
    uint32_t maxIndex_epaule_gauche;
    
    initNRF24();
    while(1) {
        if(trig == 0){
            timer.detach();
            // Init the Complex FFT module, intFlag = 0, doBitReverse = 1
            //NB using predefined arm_cfft_sR_f32_lenXXX, in this case XXX is 256
            arm_cfft_f32(&arm_cfft_sR_f32_len256, Input_ventre, 0, 1);
            arm_cfft_f32(&arm_cfft_sR_f32_len256, Input_epaule_droit, 0, 1);
            arm_cfft_f32(&arm_cfft_sR_f32_len256, Input_epaule_gauche, 0, 1);
 
            // Complex Magniture Module put results into Output(Half size of the Input)
            arm_cmplx_mag_f32(Input_ventre, Output_ventre, FFT_SIZE);
            arm_cmplx_mag_f32(Input_ventre, Output_epaule_droit, FFT_SIZE);
            arm_cmplx_mag_f32(Input_ventre, Output_epaule_gauche, FFT_SIZE);
            
            Output_ventre[0] = 0;
            Output_epaule_droit[0] = 0;
            Output_epaule_gauche[0] = 0;
            Output[0] = 0;
            //Calculates maxValue and returns corresponding value            
            arm_max_f32(Output_ventre, FFT_SIZE/2, &maxValue_ventre, &maxIndex_ventre);
            arm_max_f32(Output_epaule_droit, FFT_SIZE/2, &maxValue_epaule_droit, &maxIndex_epaule_droit);
            arm_max_f32(Output_epaule_gauche, FFT_SIZE/2, &maxValue_epaule_gauche, &maxIndex_epaule_gauche);
            
            //Envoie du nombre de point au ordinateur par bluetooth 
            dataToSend[1] = 100;
            nRF24_mod.write( NRF24L01P_PIPE_P0, dataToSend, TRANSFER_SIZE );
            
            // Calcul des points
            //Tire sur un coéquipier :
            if (maxValue_ventre<(largeur_signal_carre_equipe1+epsilon) && maxValue_ventre>(largeur_signal_carre_equipe1-epsilon) )
            {
                nbr_pts_equipe1=nbr_pts_equipe1-20; // -20pts pour l'équipe du tireur
            }
            if (maxValue_epaule_droit<(largeur_signal_carre_equipe1+epsilon) && maxValue_epaule_droit>(largeur_signal_carre_equipe1-epsilon) )
            {
                nbr_pts_equipe1=nbr_pts_equipe1-20; // -20pts pour l'équipe du tireur
            }
            if (maxValue_epaule_gauche<(largeur_signal_carre_equipe1+epsilon) && maxValue_epaule_gauche>(largeur_signal_carre_equipe1-epsilon) )
            {
                nbr_pts_equipe1=nbr_pts_equipe1-20; // -20pts pour l'équipe du tireur
            }
            
            //Tire sur le ventre   
            if (maxValue_ventre<(largeur_signal_carre_equipe2+epsilon) && maxValue_ventre>(largeur_signal_carre_equipe2-epsilon)) 
            {
                nbr_pts_equipe1=nbr_pts_equipe1-10; //Tire sur le ventre : -10pts pour l'équipe de la victime
                nbr_pts_equipe2=nbr_pts_equipe2+50; //Tire sur le ventre : +50pts pour l'équipe du tireur
            }   
            
            //Tire sur l'épaule
            if ((maxValue_epaule_droit<(largeur_signal_carre_equipe2+epsilon))&& (maxValue_epaule_droit>(largeur_signal_carre_equipe2-epsilon)) || ((maxValue_epaule_gauche<(largeur_signal_carre_equipe2+epsilon))&& (maxValue_epaule_gauche>(largeur_signal_carre_equipe2-epsilon))))
            {
                nbr_pts_equipe1=nbr_pts_equipe1-10; //Tire sur l'épaule : -10pts pour l'équipe de la victime
                nbr_pts_equipe2=nbr_pts_equipe2+100; //Tire sur l'épaule : +100pts pour l'équipe du tireur
            }
            
            // Relance de l'acquisition
            trig = 1;
            indice = 0;
            timer.attach_us(&sample,40);      //20us 50KHz sampling rate
        }
    }
}