Benoît Roussel / Mbed 2 deprecated test_algo

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

lidar.h@4:c393c14f4502, 2019-06-05 (annotated)

Committer:
Mrlinkblue
Date:
Wed Jun 05 22:55:03 2019 +0000
Revision:
4:c393c14f4502
Parent:
0:9eb40ee5ff41
Child:
5:73aac5fe9696
erreur regle sur VScode

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Mrlinkblue 0:9eb40ee5ff41 1 #include "mbed.h"
Mrlinkblue 0:9eb40ee5ff41 2
Mrlinkblue 0:9eb40ee5ff41 3 Serial ld(PC_10,PC_11,115200);
Mrlinkblue 0:9eb40ee5ff41 4
Mrlinkblue 0:9eb40ee5ff41 5 PwmOut pwm_lidar(PC_8);
Mrlinkblue 0:9eb40ee5ff41 6
Mrlinkblue 0:9eb40ee5ff41 7 void Interrupt_ldRX(void);
Mrlinkblue 0:9eb40ee5ff41 8
Mrlinkblue 0:9eb40ee5ff41 9 uint16_t data[4]; // donnees traitees
Mrlinkblue 0:9eb40ee5ff41 10 uint16_t prevData[4]; // donnees traitees precedentes
Mrlinkblue 0:9eb40ee5ff41 11 int newData = 0; // flag indiquant de nouvelles donnees
Mrlinkblue 0:9eb40ee5ff41 12 bool newRadar = false; // flag indiquant un nouveau tour effectué
Mrlinkblue 0:9eb40ee5ff41 13
Mrlinkblue 0:9eb40ee5ff41 14 // tableau tps reel : Radar[angle] = distance;
Mrlinkblue 0:9eb40ee5ff41 15 int Radar[360]; // de 0 a 359
Mrlinkblue 0:9eb40ee5ff41 16 int maxDist = 5000; // distance max visible par le lidar
Mrlinkblue 0:9eb40ee5ff41 17 int minDist = 300; // distance min visible par le lidar
Mrlinkblue 0:9eb40ee5ff41 18
Mrlinkblue 0:9eb40ee5ff41 19 // commandes
Mrlinkblue 0:9eb40ee5ff41 20 char ldStart[3] = {0xA5, 0x20,0};
Mrlinkblue 0:9eb40ee5ff41 21 char ldStop[3] = {0xA5, 0x25, 0};
Mrlinkblue 0:9eb40ee5ff41 22 char ldReset[3] = {0xA5, 0x40, 0};
Mrlinkblue 0:9eb40ee5ff41 23 // donnees brutes recues au debut
Mrlinkblue 0:9eb40ee5ff41 24 char start_data_received[7];
Mrlinkblue 0:9eb40ee5ff41 25
Mrlinkblue 0:9eb40ee5ff41 26 void recuperer_start(char* data_received){
Mrlinkblue 0:9eb40ee5ff41 27 // recupere le premier paquet, qui est different des suivants
Mrlinkblue 0:9eb40ee5ff41 28 int i=0;
Mrlinkblue 0:9eb40ee5ff41 29 while(i<7){
Mrlinkblue 0:9eb40ee5ff41 30 data_received[i] = ld.getc();
Mrlinkblue 0:9eb40ee5ff41 31 if((i!=0)||(i==0 && data_received[i]==0xA5)){
Mrlinkblue 0:9eb40ee5ff41 32 i++;
Mrlinkblue 0:9eb40ee5ff41 33 }
Mrlinkblue 0:9eb40ee5ff41 34 }
Mrlinkblue 0:9eb40ee5ff41 35 }
Mrlinkblue 0:9eb40ee5ff41 36
Mrlinkblue 0:9eb40ee5ff41 37
Mrlinkblue 0:9eb40ee5ff41 38 void ldConvert(uint8_t* data_received, uint16_t* data){
Mrlinkblue 0:9eb40ee5ff41 39 // convertit les donnees recues : 5 paquets de 2 hexas
Mrlinkblue 0:9eb40ee5ff41 40 uint8_t qualityHex;
Mrlinkblue 0:9eb40ee5ff41 41 uint16_t checkS;
Mrlinkblue 0:9eb40ee5ff41 42 uint16_t quality;
Mrlinkblue 0:9eb40ee5ff41 43 // angle + checkC
Mrlinkblue 0:9eb40ee5ff41 44 uint16_t angleHex1 ;
Mrlinkblue 0:9eb40ee5ff41 45 uint16_t angleHex2;
Mrlinkblue 0:9eb40ee5ff41 46 uint16_t checkC ;
Mrlinkblue 0:9eb40ee5ff41 47 uint8_t distanceHex2,distanceHex1;
Mrlinkblue 0:9eb40ee5ff41 48 uint16_t angle,distance;
Mrlinkblue 0:9eb40ee5ff41 49 // quality + checkS
Mrlinkblue 0:9eb40ee5ff41 50 qualityHex = data_received[0];
Mrlinkblue 0:9eb40ee5ff41 51 checkS = ((qualityHex&0x02)>>1)!=(qualityHex&0x01);
Mrlinkblue 0:9eb40ee5ff41 52 quality = (qualityHex&0xFC)>>2;
Mrlinkblue 0:9eb40ee5ff41 53 // angle + checkC
Mrlinkblue 0:9eb40ee5ff41 54 angleHex1 = data_received[1];
Mrlinkblue 0:9eb40ee5ff41 55 angleHex2 = data_received[2];
Mrlinkblue 0:9eb40ee5ff41 56 checkC = (angleHex1&0x01);
Mrlinkblue 0:9eb40ee5ff41 57 angle = (((uint16_t)angleHex2) <<1) + ((uint16_t)angleHex1>>7);
Mrlinkblue 4:c393c14f4502 58 angle = angle - 180; // Decalage par rapport au centre du lidar et de la voiture
Mrlinkblue 0:9eb40ee5ff41 59 // distance
Mrlinkblue 0:9eb40ee5ff41 60 distanceHex1 = data_received[3];
Mrlinkblue 0:9eb40ee5ff41 61 distanceHex2 = data_received[4];
Mrlinkblue 0:9eb40ee5ff41 62 distance = (((uint16_t)distanceHex2) <<6) + (distanceHex1>>2);
Mrlinkblue 0:9eb40ee5ff41 63 data[0] = (checkS && checkC);
Mrlinkblue 0:9eb40ee5ff41 64 data[1] = quality;
Mrlinkblue 0:9eb40ee5ff41 65 if(angle >= 360){
Mrlinkblue 0:9eb40ee5ff41 66 angle = 0;
Mrlinkblue 0:9eb40ee5ff41 67 }
Mrlinkblue 0:9eb40ee5ff41 68 data[2] = angle;
Mrlinkblue 0:9eb40ee5ff41 69 data[3] = distance;
Mrlinkblue 0:9eb40ee5ff41 70 }
Mrlinkblue 0:9eb40ee5ff41 71
Mrlinkblue 0:9eb40ee5ff41 72
Mrlinkblue 0:9eb40ee5ff41 73 void addRadar(int* Radar, uint16_t* data, uint16_t* prevData){
Mrlinkblue 0:9eb40ee5ff41 74 // ajoute les donnees du bloc data dans le radar actuel
Mrlinkblue 0:9eb40ee5ff41 75 int cA = data[2];
Mrlinkblue 0:9eb40ee5ff41 76 int cD = data[3];
Mrlinkblue 0:9eb40ee5ff41 77 int pA = prevData[2];
Mrlinkblue 0:9eb40ee5ff41 78 int pD = prevData[3];
Mrlinkblue 0:9eb40ee5ff41 79 // pente de la droite d interpolation lineaire
Mrlinkblue 0:9eb40ee5ff41 80 float coeff;
Mrlinkblue 0:9eb40ee5ff41 81 // si on a pas fait un tour complet
Mrlinkblue 0:9eb40ee5ff41 82 if(cA >= pA){
Mrlinkblue 0:9eb40ee5ff41 83 newRadar = false;
Mrlinkblue 0:9eb40ee5ff41 84 coeff = (float)(cD-pD)/(float)(cA-pA);
Mrlinkblue 0:9eb40ee5ff41 85 int ka = pA + 1;
Mrlinkblue 0:9eb40ee5ff41 86 while(ka < cA){
Mrlinkblue 0:9eb40ee5ff41 87 float distance = (float)pD + coeff*(float)(ka - pA);
Mrlinkblue 0:9eb40ee5ff41 88 if(distance > maxDist){
Mrlinkblue 0:9eb40ee5ff41 89 distance = maxDist;
Mrlinkblue 0:9eb40ee5ff41 90 }else if(distance < minDist){
Mrlinkblue 0:9eb40ee5ff41 91 distance = minDist;
Mrlinkblue 0:9eb40ee5ff41 92 }
Mrlinkblue 0:9eb40ee5ff41 93 Radar[ka] = distance;
Mrlinkblue 0:9eb40ee5ff41 94 ++ka;
Mrlinkblue 0:9eb40ee5ff41 95 }
Mrlinkblue 0:9eb40ee5ff41 96 }
Mrlinkblue 0:9eb40ee5ff41 97 else{
Mrlinkblue 0:9eb40ee5ff41 98 // si on a fait un tour
Mrlinkblue 0:9eb40ee5ff41 99 newRadar = true;
Mrlinkblue 0:9eb40ee5ff41 100 int ka = pA + 1;
Mrlinkblue 0:9eb40ee5ff41 101 coeff = (float)(cD-pD)/(float)(cA+360-pA);
Mrlinkblue 0:9eb40ee5ff41 102 // fin du Radar
Mrlinkblue 0:9eb40ee5ff41 103 while(ka < 360){
Mrlinkblue 0:9eb40ee5ff41 104 float distance = (float)pD + coeff*(float)(ka - pA);
Mrlinkblue 0:9eb40ee5ff41 105 if(distance > maxDist){
Mrlinkblue 0:9eb40ee5ff41 106 distance = maxDist;
Mrlinkblue 0:9eb40ee5ff41 107 }else if(distance < minDist){
Mrlinkblue 0:9eb40ee5ff41 108 distance = minDist;
Mrlinkblue 0:9eb40ee5ff41 109 }
Mrlinkblue 0:9eb40ee5ff41 110 Radar[ka] = distance;
Mrlinkblue 0:9eb40ee5ff41 111 ++ka;
Mrlinkblue 0:9eb40ee5ff41 112 }
Mrlinkblue 0:9eb40ee5ff41 113 // début du Radar
Mrlinkblue 0:9eb40ee5ff41 114 ka = 0;
Mrlinkblue 0:9eb40ee5ff41 115 while(ka <= cA){
Mrlinkblue 0:9eb40ee5ff41 116 Radar[ka] = Radar[359] + coeff*(ka + 1);
Mrlinkblue 0:9eb40ee5ff41 117 ++ka;
Mrlinkblue 0:9eb40ee5ff41 118 }
Mrlinkblue 0:9eb40ee5ff41 119 }
Mrlinkblue 0:9eb40ee5ff41 120 }
Mrlinkblue 0:9eb40ee5ff41 121
Mrlinkblue 0:9eb40ee5ff41 122 void Interrupt_ldRX(void){
Mrlinkblue 0:9eb40ee5ff41 123 static int i=0; // indice dans un paquet de donnees (5 octets)
Mrlinkblue 0:9eb40ee5ff41 124 static uint8_t data_received_temp[5];
Mrlinkblue 0:9eb40ee5ff41 125 uint16_t data[4];
Mrlinkblue 0:9eb40ee5ff41 126 //static uint16_t prevData[4];
Mrlinkblue 0:9eb40ee5ff41 127 //static int newData = 0;
Mrlinkblue 0:9eb40ee5ff41 128
Mrlinkblue 0:9eb40ee5ff41 129 data_received_temp[i] = ld.getc(); // on recup l info
Mrlinkblue 0:9eb40ee5ff41 130
Mrlinkblue 0:9eb40ee5ff41 131 // si on atteint le fin du paquet de donnees
Mrlinkblue 0:9eb40ee5ff41 132 if(i==4){
Mrlinkblue 0:9eb40ee5ff41 133 // on met les donnees utilisables dans data
Mrlinkblue 0:9eb40ee5ff41 134 ldConvert(data_received_temp, data);
Mrlinkblue 0:9eb40ee5ff41 135 // on verifie le bit de check et la qualite
Mrlinkblue 0:9eb40ee5ff41 136 if (data[0] &&(data[1]>=8)){
Mrlinkblue 0:9eb40ee5ff41 137 // data -> prevData
Mrlinkblue 0:9eb40ee5ff41 138 for(int k=0; k<4; k++){
Mrlinkblue 0:9eb40ee5ff41 139 prevData[k] = data[k];
Mrlinkblue 0:9eb40ee5ff41 140 }
Mrlinkblue 0:9eb40ee5ff41 141 newData = 1;
Mrlinkblue 0:9eb40ee5ff41 142 }
Mrlinkblue 0:9eb40ee5ff41 143 if (data[0] &&(data[1]<8)){
Mrlinkblue 0:9eb40ee5ff41 144 for(int k=0; k<3; k++){
Mrlinkblue 0:9eb40ee5ff41 145 prevData[k] = data[k];
Mrlinkblue 0:9eb40ee5ff41 146 }
Mrlinkblue 0:9eb40ee5ff41 147 newData = 1;
Mrlinkblue 0:9eb40ee5ff41 148 }
Mrlinkblue 0:9eb40ee5ff41 149 }
Mrlinkblue 0:9eb40ee5ff41 150 i=(i+1)%5;
Mrlinkblue 0:9eb40ee5ff41 151 }
Mrlinkblue 0:9eb40ee5ff41 152
Mrlinkblue 0:9eb40ee5ff41 153 void setup_lidar(){
Mrlinkblue 0:9eb40ee5ff41 154 // start lidar
Mrlinkblue 0:9eb40ee5ff41 155 wait(0.5);
Mrlinkblue 0:9eb40ee5ff41 156 pwm_lidar.period_us(40);
Mrlinkblue 0:9eb40ee5ff41 157 pwm_lidar.write(0.5);
Mrlinkblue 0:9eb40ee5ff41 158 wait(1);
Mrlinkblue 0:9eb40ee5ff41 159 ld.puts(ldStart);
Mrlinkblue 0:9eb40ee5ff41 160 // on recupere le paquet de depart
Mrlinkblue 0:9eb40ee5ff41 161 recuperer_start(start_data_received);
Mrlinkblue 0:9eb40ee5ff41 162 wait(0.1);
Mrlinkblue 0:9eb40ee5ff41 163 // fonction d interruption
Mrlinkblue 0:9eb40ee5ff41 164 ld.attach(Interrupt_ldRX,Serial::RxIrq);
Mrlinkblue 0:9eb40ee5ff41 165 }