prog robocup equip2

Dependencies:   nRF24L01P MX12 BSP_B-L475E-IOT01

main.cpp

Committer:
chedozeauclement
Date:
2021-04-23
Revision:
1:2bc2848b78d1
Parent:
0:7b8f377e2077

File content as of revision 1:2bc2848b78d1:

/*
TRANSFER_SIZE = 24
id_robot, vx, vy, angle_robot, vangulaire, v_tir, cmd_rouleau 
*/

#include "mbed.h"
#include "MX12.h"
#include <iostream>
#include "stm32l475e_iot01_magneto.h"
#include "Projet.h"
#include "nRF24L01P.h"
#include "string.h"

static UnbufferedSerial pc(USBTX, USBRX,115200); // tx, rx

nRF24L01P my_nrf24l01p(D11, D12, D13, D9, D10, D8);    // mosi, miso, sck, csn, ce, irq

DigitalOut myled2(LED2);

DigitalOut led(LED1);
MX12 mx (PC_4, PC_5,115200);

int main()
{
    
// Nombre de caractères transmis à chaque envoi
#define TRANSFER_SIZE   24
    char texte[100];
    char txData[TRANSFER_SIZE]= {};
    char rxData[TRANSFER_SIZE+1]= {};
    char caractere_recu=0;
    int txDataCnt = 0;
    int rxDataCnt = 0;
    int statut_nrF;
    int id_robot, vx, vy, angle_robot, vangulaire, v_tir, cmd_rouleau;
    int consigneData[6];

    pc.baud(115200); // Débit de la liaison série PC
    
    my_nrf24l01p.powerUp();

    my_nrf24l01p.setRfFrequency(2414);
    my_nrf24l01p.setRxAddress(0xA1A1A1A1A1);
    my_nrf24l01p.setTxAddress(0xB1A1A1A1A1);
    my_nrf24l01p.setAirDataRate(NRF24L01P_DATARATE_1_MBPS);

    my_nrf24l01p.setTransferSize( TRANSFER_SIZE );
    my_nrf24l01p.setRfOutputPower(-6);
    
    
    sprintf(texte,"DISCO\r\n");
    pc.write(texte,strlen(texte));
    
    sprintf(texte,"nRF24L01+ Frequency    : %d MHz\r\n",  my_nrf24l01p.getRfFrequency());
    pc.write(texte,strlen(texte));
    
    sprintf(texte,"nRF24L01+ Output power : %d dBm\r\n",  my_nrf24l01p.getRfOutputPower() );
    pc.write(texte,strlen(texte));
    
    sprintf(texte,"nRF24L01+ Data Rate    : %d kbps\r\n", my_nrf24l01p.getAirDataRate()  );
    pc.write(texte,strlen(texte));
    
    sprintf(texte,"nRF24L01+ TX Address   : 0x%010llX\r\n", my_nrf24l01p.getTxAddress() );
    pc.write(texte,strlen(texte));
    
    sprintf(texte,"nRF24L01+ RX Address   : 0x%010llX\r\n", my_nrf24l01p.getRxAddress()  );
    pc.write(texte,strlen(texte));


    my_nrf24l01p.setTransferSize( TRANSFER_SIZE );
    my_nrf24l01p.setReceiveMode();
    my_nrf24l01p.enable();
  

    sprintf(texte,"Pret a recevoir, led2 ON\n\r" );
    pc.write(texte,strlen(texte));

    myled2=!myled2;

    while(1) {
        
    // RECEPTION DE DONNEES ------------------------------------------------------------------
        
        for(int k=0;k<10000000;k++);
        statut_nrF=my_nrf24l01p.readable(NRF24L01P_PIPE_P0);
        
        if ( statut_nrF != 0) {
            
            myled2 = !myled2;
            
            sprintf(texte,"Reception\n\r");
            pc.write(texte,strlen(texte));
 
            rxDataCnt = my_nrf24l01p.read( NRF24L01P_PIPE_P0, rxData, TRANSFER_SIZE );
            
            //Traitement donnees--------------------------------
            
            if(sscanf(rxData, "%d,%d,%d,%d,%d,%d,%d",&id_robot,&vx,&vy,&angle_robot,&vangulaire,&v_tir,&cmd_rouleau)==7){ // lecture et parsing de la chaîne
            
            
            sprintf(texte,"Valeur de id_robot : %d \r\n",id_robot);
            pc.write(texte,strlen(texte));
            
            sprintf(texte,"Valeur de vx : %d \r\n",vx);
            pc.write(texte,strlen(texte));
            
            sprintf(texte,"Valeur de vy : %d \r\n",vy);
            pc.write(texte,strlen(texte));
            
            sprintf(texte,"Valeur de angle_robot : %d \r\n",angle_robot);
            pc.write(texte,strlen(texte));

            sprintf(texte,"Valeur de vangulaire : %d \r\n",vangulaire);
            pc.write(texte,strlen(texte));

            sprintf(texte,"Valeur de v_tir : %d \r\n",v_tir);
            pc.write(texte,strlen(texte));

            sprintf(texte,"Valeur de cmd_rouleau : %d \r\n",cmd_rouleau);
            pc.write(texte,strlen(texte));
            
            
            }
            
            else {sprintf(texte,rxData);
            pc.write(texte,strlen(texte));}
            
            
            consigneData[0] = id_robot;
            consigneData[1] = vx;
            consigneData[2] = vy;
            consigneData[3] = angle_robot;
            consigneData[4] = vangulaire;
            consigneData[5] = v_tir;
            consigneData[6] = cmd_rouleau;


            sprintf(texte,rxData);
            pc.write(texte,strlen(texte));

            sprintf(texte,"   Taille %d\r\n",rxDataCnt);
            pc.write(texte,strlen(texte));
            

        led = 1;
        cmd_inverse(vx,vy,vangulaire,angle_robot,&mx);

        }
    }
}