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@7:20d05f0d11a2, 2019-06-08 (annotated)

Committer:
Mrlinkblue
Date:
Sat Jun 08 09:41:48 2019 +0000
Revision:
7:20d05f0d11a2
Parent:
6:9af875ef7b30 
version_8_juin

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