IOGS / LaserGame / Project
Dependencies: mbed nRF24L01P mbed-dsp
main.cpp
- Committer:
- villemejane
- Date:
- 2022-03-21
- Revision:
- 3:627354c99ba0
- Parent:
- 1:47d90ce030a3
- Child:
- 4:5c5298ceb3a3
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 } } }