Robocup5 / Mbed OS DISCO_L475VG_IOT01-Sensors-BSP

prog robocup equip2

Dependencies:   nRF24L01P MX12 BSP_B-L475E-IOT01

main.cpp@1:2bc2848b78d1, 2021-04-23 (annotated)

Committer:
chedozeauclement
Date:
Fri Apr 23 09:08:02 2021 +0000
Revision:
1:2bc2848b78d1
Parent:
0:7b8f377e2077 
Prog comm/mvt;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chedozeauclement 0:7b8f377e2077 1 /*
chedozeauclement 0:7b8f377e2077 2 TRANSFER_SIZE = 24
chedozeauclement 0:7b8f377e2077 3 id_robot, vx, vy, angle_robot, vangulaire, v_tir, cmd_rouleau
chedozeauclement 0:7b8f377e2077 4 */
chedozeauclement 0:7b8f377e2077 5
chedozeauclement 0:7b8f377e2077 6 #include "mbed.h"
chedozeauclement 0:7b8f377e2077 7 #include "MX12.h"
chedozeauclement 0:7b8f377e2077 8 #include <iostream>
chedozeauclement 0:7b8f377e2077 9 #include "stm32l475e_iot01_magneto.h"
chedozeauclement 0:7b8f377e2077 10 #include "Projet.h"
chedozeauclement 0:7b8f377e2077 11 #include "nRF24L01P.h"
chedozeauclement 1:2bc2848b78d1 12 #include "string.h"
chedozeauclement 0:7b8f377e2077 13
chedozeauclement 1:2bc2848b78d1 14 static UnbufferedSerial pc(USBTX, USBRX,115200); // tx, rx
chedozeauclement 0:7b8f377e2077 15
chedozeauclement 0:7b8f377e2077 16 nRF24L01P my_nrf24l01p(D11, D12, D13, D9, D10, D8); // mosi, miso, sck, csn, ce, irq
chedozeauclement 0:7b8f377e2077 17
chedozeauclement 0:7b8f377e2077 18 DigitalOut myled2(LED2);
chedozeauclement 0:7b8f377e2077 19
chedozeauclement 0:7b8f377e2077 20 DigitalOut led(LED1);
chedozeauclement 0:7b8f377e2077 21 MX12 mx (PC_4, PC_5,115200);
chedozeauclement 0:7b8f377e2077 22
chedozeauclement 0:7b8f377e2077 23 int main()
chedozeauclement 0:7b8f377e2077 24 {
chedozeauclement 0:7b8f377e2077 25
chedozeauclement 0:7b8f377e2077 26 // Nombre de caractères transmis à chaque envoi
chedozeauclement 0:7b8f377e2077 27 #define TRANSFER_SIZE 24
chedozeauclement 1:2bc2848b78d1 28 char texte[100];
chedozeauclement 0:7b8f377e2077 29 char txData[TRANSFER_SIZE]= {};
chedozeauclement 0:7b8f377e2077 30 char rxData[TRANSFER_SIZE+1]= {};
chedozeauclement 0:7b8f377e2077 31 char caractere_recu=0;
chedozeauclement 0:7b8f377e2077 32 int txDataCnt = 0;
chedozeauclement 0:7b8f377e2077 33 int rxDataCnt = 0;
chedozeauclement 0:7b8f377e2077 34 int statut_nrF;
chedozeauclement 0:7b8f377e2077 35 int id_robot, vx, vy, angle_robot, vangulaire, v_tir, cmd_rouleau;
chedozeauclement 0:7b8f377e2077 36 int consigneData[6];
chedozeauclement 0:7b8f377e2077 37
chedozeauclement 0:7b8f377e2077 38 pc.baud(115200); // Débit de la liaison série PC
chedozeauclement 0:7b8f377e2077 39
chedozeauclement 0:7b8f377e2077 40 my_nrf24l01p.powerUp();
chedozeauclement 0:7b8f377e2077 41
chedozeauclement 0:7b8f377e2077 42 my_nrf24l01p.setRfFrequency(2414);
chedozeauclement 0:7b8f377e2077 43 my_nrf24l01p.setRxAddress(0xA1A1A1A1A1);
chedozeauclement 0:7b8f377e2077 44 my_nrf24l01p.setTxAddress(0xB1A1A1A1A1);
chedozeauclement 0:7b8f377e2077 45 my_nrf24l01p.setAirDataRate(NRF24L01P_DATARATE_1_MBPS);
chedozeauclement 0:7b8f377e2077 46
chedozeauclement 0:7b8f377e2077 47 my_nrf24l01p.setTransferSize( TRANSFER_SIZE );
chedozeauclement 0:7b8f377e2077 48 my_nrf24l01p.setRfOutputPower(-6);
chedozeauclement 0:7b8f377e2077 49
chedozeauclement 1:2bc2848b78d1 50
chedozeauclement 1:2bc2848b78d1 51 sprintf(texte,"DISCO\r\n");
chedozeauclement 1:2bc2848b78d1 52 pc.write(texte,strlen(texte));
chedozeauclement 1:2bc2848b78d1 53
chedozeauclement 1:2bc2848b78d1 54 sprintf(texte,"nRF24L01+ Frequency : %d MHz\r\n", my_nrf24l01p.getRfFrequency());
chedozeauclement 1:2bc2848b78d1 55 pc.write(texte,strlen(texte));
chedozeauclement 1:2bc2848b78d1 56
chedozeauclement 1:2bc2848b78d1 57 sprintf(texte,"nRF24L01+ Output power : %d dBm\r\n", my_nrf24l01p.getRfOutputPower() );
chedozeauclement 1:2bc2848b78d1 58 pc.write(texte,strlen(texte));
chedozeauclement 1:2bc2848b78d1 59
chedozeauclement 1:2bc2848b78d1 60 sprintf(texte,"nRF24L01+ Data Rate : %d kbps\r\n", my_nrf24l01p.getAirDataRate() );
chedozeauclement 1:2bc2848b78d1 61 pc.write(texte,strlen(texte));
chedozeauclement 1:2bc2848b78d1 62
chedozeauclement 1:2bc2848b78d1 63 sprintf(texte,"nRF24L01+ TX Address : 0x%010llX\r\n", my_nrf24l01p.getTxAddress() );
chedozeauclement 1:2bc2848b78d1 64 pc.write(texte,strlen(texte));
chedozeauclement 1:2bc2848b78d1 65
chedozeauclement 1:2bc2848b78d1 66 sprintf(texte,"nRF24L01+ RX Address : 0x%010llX\r\n", my_nrf24l01p.getRxAddress() );
chedozeauclement 1:2bc2848b78d1 67 pc.write(texte,strlen(texte));
chedozeauclement 0:7b8f377e2077 68
chedozeauclement 0:7b8f377e2077 69
chedozeauclement 0:7b8f377e2077 70 my_nrf24l01p.setTransferSize( TRANSFER_SIZE );
chedozeauclement 0:7b8f377e2077 71 my_nrf24l01p.setReceiveMode();
chedozeauclement 0:7b8f377e2077 72 my_nrf24l01p.enable();
chedozeauclement 1:2bc2848b78d1 73
chedozeauclement 1:2bc2848b78d1 74
chedozeauclement 1:2bc2848b78d1 75 sprintf(texte,"Pret a recevoir, led2 ON\n\r" );
chedozeauclement 1:2bc2848b78d1 76 pc.write(texte,strlen(texte));
chedozeauclement 1:2bc2848b78d1 77
chedozeauclement 0:7b8f377e2077 78 myled2=!myled2;
chedozeauclement 0:7b8f377e2077 79
chedozeauclement 0:7b8f377e2077 80 while(1) {
chedozeauclement 0:7b8f377e2077 81
chedozeauclement 0:7b8f377e2077 82 // RECEPTION DE DONNEES ------------------------------------------------------------------
chedozeauclement 0:7b8f377e2077 83
chedozeauclement 0:7b8f377e2077 84 for(int k=0;k<10000000;k++);
chedozeauclement 0:7b8f377e2077 85 statut_nrF=my_nrf24l01p.readable(NRF24L01P_PIPE_P0);
chedozeauclement 0:7b8f377e2077 86
chedozeauclement 0:7b8f377e2077 87 if ( statut_nrF != 0) {
chedozeauclement 0:7b8f377e2077 88
chedozeauclement 0:7b8f377e2077 89 myled2 = !myled2;
chedozeauclement 1:2bc2848b78d1 90
chedozeauclement 1:2bc2848b78d1 91 sprintf(texte,"Reception\n\r");
chedozeauclement 1:2bc2848b78d1 92 pc.write(texte,strlen(texte));
chedozeauclement 0:7b8f377e2077 93
chedozeauclement 0:7b8f377e2077 94 rxDataCnt = my_nrf24l01p.read( NRF24L01P_PIPE_P0, rxData, TRANSFER_SIZE );
chedozeauclement 0:7b8f377e2077 95
chedozeauclement 0:7b8f377e2077 96 //Traitement donnees--------------------------------
chedozeauclement 0:7b8f377e2077 97
chedozeauclement 0:7b8f377e2077 98 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
chedozeauclement 1:2bc2848b78d1 99
chedozeauclement 1:2bc2848b78d1 100
chedozeauclement 1:2bc2848b78d1 101 sprintf(texte,"Valeur de id_robot : %d \r\n",id_robot);
chedozeauclement 1:2bc2848b78d1 102 pc.write(texte,strlen(texte));
chedozeauclement 1:2bc2848b78d1 103
chedozeauclement 1:2bc2848b78d1 104 sprintf(texte,"Valeur de vx : %d \r\n",vx);
chedozeauclement 1:2bc2848b78d1 105 pc.write(texte,strlen(texte));
chedozeauclement 1:2bc2848b78d1 106
chedozeauclement 1:2bc2848b78d1 107 sprintf(texte,"Valeur de vy : %d \r\n",vy);
chedozeauclement 1:2bc2848b78d1 108 pc.write(texte,strlen(texte));
chedozeauclement 1:2bc2848b78d1 109
chedozeauclement 1:2bc2848b78d1 110 sprintf(texte,"Valeur de angle_robot : %d \r\n",angle_robot);
chedozeauclement 1:2bc2848b78d1 111 pc.write(texte,strlen(texte));
chedozeauclement 1:2bc2848b78d1 112
chedozeauclement 1:2bc2848b78d1 113 sprintf(texte,"Valeur de vangulaire : %d \r\n",vangulaire);
chedozeauclement 1:2bc2848b78d1 114 pc.write(texte,strlen(texte));
chedozeauclement 1:2bc2848b78d1 115
chedozeauclement 1:2bc2848b78d1 116 sprintf(texte,"Valeur de v_tir : %d \r\n",v_tir);
chedozeauclement 1:2bc2848b78d1 117 pc.write(texte,strlen(texte));
chedozeauclement 1:2bc2848b78d1 118
chedozeauclement 1:2bc2848b78d1 119 sprintf(texte,"Valeur de cmd_rouleau : %d \r\n",cmd_rouleau);
chedozeauclement 1:2bc2848b78d1 120 pc.write(texte,strlen(texte));
chedozeauclement 1:2bc2848b78d1 121
chedozeauclement 1:2bc2848b78d1 122
chedozeauclement 0:7b8f377e2077 123 }
chedozeauclement 0:7b8f377e2077 124
chedozeauclement 1:2bc2848b78d1 125 else {sprintf(texte,rxData);
chedozeauclement 1:2bc2848b78d1 126 pc.write(texte,strlen(texte));}
chedozeauclement 1:2bc2848b78d1 127
chedozeauclement 0:7b8f377e2077 128
chedozeauclement 0:7b8f377e2077 129 consigneData[0] = id_robot;
chedozeauclement 0:7b8f377e2077 130 consigneData[1] = vx;
chedozeauclement 0:7b8f377e2077 131 consigneData[2] = vy;
chedozeauclement 0:7b8f377e2077 132 consigneData[3] = angle_robot;
chedozeauclement 0:7b8f377e2077 133 consigneData[4] = vangulaire;
chedozeauclement 0:7b8f377e2077 134 consigneData[5] = v_tir;
chedozeauclement 0:7b8f377e2077 135 consigneData[6] = cmd_rouleau;
chedozeauclement 1:2bc2848b78d1 136
chedozeauclement 0:7b8f377e2077 137
chedozeauclement 1:2bc2848b78d1 138 sprintf(texte,rxData);
chedozeauclement 1:2bc2848b78d1 139 pc.write(texte,strlen(texte));
chedozeauclement 1:2bc2848b78d1 140
chedozeauclement 1:2bc2848b78d1 141 sprintf(texte," Taille %d\r\n",rxDataCnt);
chedozeauclement 1:2bc2848b78d1 142 pc.write(texte,strlen(texte));
chedozeauclement 1:2bc2848b78d1 143
chedozeauclement 0:7b8f377e2077 144
chedozeauclement 0:7b8f377e2077 145 led = 1;
chedozeauclement 0:7b8f377e2077 146 cmd_inverse(vx,vy,vangulaire,angle_robot,&mx);
chedozeauclement 0:7b8f377e2077 147
chedozeauclement 0:7b8f377e2077 148 }
chedozeauclement 0:7b8f377e2077 149 }
chedozeauclement 0:7b8f377e2077 150 }