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@6:9af875ef7b30, 2019-06-06 (annotated)

Committer:
Mrlinkblue
Date:
Thu Jun 06 16:30:59 2019 +0000
Revision:
6:9af875ef7b30
Parent:
5:73aac5fe9696
Child:
7:20d05f0d11a2
Version_jeudi_6_soir

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 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 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 6:9af875ef7b30 49 myled1=!myled1;
Mrlinkblue 0:9eb40ee5ff41 50 // quality + checkS
Mrlinkblue 0:9eb40ee5ff41 51 qualityHex = data_received[0];
Mrlinkblue 0:9eb40ee5ff41 52 checkS = ((qualityHex&0x02)>>1)!=(qualityHex&0x01);
Mrlinkblue 0:9eb40ee5ff41 53 quality = (qualityHex&0xFC)>>2;
Mrlinkblue 0:9eb40ee5ff41 54 // angle + checkC
Mrlinkblue 0:9eb40ee5ff41 55 angleHex1 = data_received[1];
Mrlinkblue 0:9eb40ee5ff41 56 angleHex2 = data_received[2];
Mrlinkblue 0:9eb40ee5ff41 57 checkC = (angleHex1&0x01);
Mrlinkblue 0:9eb40ee5ff41 58 angle = (((uint16_t)angleHex2) <<1) + ((uint16_t)angleHex1>>7);
Mrlinkblue 4:c393c14f4502 59 angle = angle - 180; // Decalage par rapport au centre du lidar et de la voiture
Mrlinkblue 0:9eb40ee5ff41 60 // distance
Mrlinkblue 0:9eb40ee5ff41 61 distanceHex1 = data_received[3];
Mrlinkblue 0:9eb40ee5ff41 62 distanceHex2 = data_received[4];
Mrlinkblue 0:9eb40ee5ff41 63 distance = (((uint16_t)distanceHex2) <<6) + (distanceHex1>>2);
Mrlinkblue 0:9eb40ee5ff41 64 data[0] = (checkS && checkC);
Mrlinkblue 0:9eb40ee5ff41 65 data[1] = quality;
Mrlinkblue 0:9eb40ee5ff41 66 if(angle >= 360){
Mrlinkblue 0:9eb40ee5ff41 67 angle = 0;
Mrlinkblue 0:9eb40ee5ff41 68 }
Mrlinkblue 0:9eb40ee5ff41 69 data[2] = angle;
Mrlinkblue 0:9eb40ee5ff41 70 data[3] = distance;
Mrlinkblue 0:9eb40ee5ff41 71 }
Mrlinkblue 0:9eb40ee5ff41 72
Mrlinkblue 0:9eb40ee5ff41 73
Mrlinkblue 6:9af875ef7b30 74 void addRadar(uint16_t* data, uint16_t* prevData){
Mrlinkblue 0:9eb40ee5ff41 75 // ajoute les donnees du bloc data dans le radar actuel
Mrlinkblue 0:9eb40ee5ff41 76 int cA = data[2];
Mrlinkblue 0:9eb40ee5ff41 77 int cD = data[3];
Mrlinkblue 0:9eb40ee5ff41 78 int pA = prevData[2];
Mrlinkblue 0:9eb40ee5ff41 79 int pD = prevData[3];
Mrlinkblue 0:9eb40ee5ff41 80 // pente de la droite d interpolation lineaire
Mrlinkblue 0:9eb40ee5ff41 81 float coeff;
Mrlinkblue 0:9eb40ee5ff41 82 // si on a pas fait un tour complet
Mrlinkblue 0:9eb40ee5ff41 83 if(cA >= pA){
Mrlinkblue 5:73aac5fe9696 84
Mrlinkblue 0:9eb40ee5ff41 85 coeff = (float)(cD-pD)/(float)(cA-pA);
Mrlinkblue 0:9eb40ee5ff41 86 int ka = pA + 1;
Mrlinkblue 0:9eb40ee5ff41 87 while(ka < cA){
Mrlinkblue 0:9eb40ee5ff41 88 float distance = (float)pD + coeff*(float)(ka - pA);
Mrlinkblue 0:9eb40ee5ff41 89 if(distance > maxDist){
Mrlinkblue 0:9eb40ee5ff41 90 distance = maxDist;
Mrlinkblue 0:9eb40ee5ff41 91 }else if(distance < minDist){
Mrlinkblue 0:9eb40ee5ff41 92 distance = minDist;
Mrlinkblue 0:9eb40ee5ff41 93 }
Mrlinkblue 0:9eb40ee5ff41 94 Radar[ka] = distance;
Mrlinkblue 0:9eb40ee5ff41 95 ++ka;
Mrlinkblue 0:9eb40ee5ff41 96 }
Mrlinkblue 6:9af875ef7b30 97 Radar[cA] = cD;
Mrlinkblue 0:9eb40ee5ff41 98 }
Mrlinkblue 6:9af875ef7b30 99 else{ //Pa>ca
Mrlinkblue 0:9eb40ee5ff41 100 // si on a fait un tour
Mrlinkblue 0:9eb40ee5ff41 101 newRadar = true;
Mrlinkblue 0:9eb40ee5ff41 102 int ka = pA + 1;
Mrlinkblue 0:9eb40ee5ff41 103 coeff = (float)(cD-pD)/(float)(cA+360-pA);
Mrlinkblue 0:9eb40ee5ff41 104 // fin du Radar
Mrlinkblue 0:9eb40ee5ff41 105 while(ka < 360){
Mrlinkblue 0:9eb40ee5ff41 106 float distance = (float)pD + coeff*(float)(ka - pA);
Mrlinkblue 0:9eb40ee5ff41 107 if(distance > maxDist){
Mrlinkblue 0:9eb40ee5ff41 108 distance = maxDist;
Mrlinkblue 5:73aac5fe9696 109 }
Mrlinkblue 5:73aac5fe9696 110 else if(distance < minDist){
Mrlinkblue 0:9eb40ee5ff41 111 distance = minDist;
Mrlinkblue 0:9eb40ee5ff41 112 }
Mrlinkblue 0:9eb40ee5ff41 113 Radar[ka] = distance;
Mrlinkblue 0:9eb40ee5ff41 114 ++ka;
Mrlinkblue 0:9eb40ee5ff41 115 }
Mrlinkblue 0:9eb40ee5ff41 116 // début du Radar
Mrlinkblue 0:9eb40ee5ff41 117 ka = 0;
Mrlinkblue 0:9eb40ee5ff41 118 while(ka <= cA){
Mrlinkblue 0:9eb40ee5ff41 119 Radar[ka] = Radar[359] + coeff*(ka + 1);
Mrlinkblue 0:9eb40ee5ff41 120 ++ka;
Mrlinkblue 0:9eb40ee5ff41 121 }
Mrlinkblue 0:9eb40ee5ff41 122 }
Mrlinkblue 0:9eb40ee5ff41 123 }
Mrlinkblue 0:9eb40ee5ff41 124
Mrlinkblue 0:9eb40ee5ff41 125 void Interrupt_ldRX(void){
Mrlinkblue 6:9af875ef7b30 126 // Toutes les 500 microsecondes
Mrlinkblue 0:9eb40ee5ff41 127 static int i=0; // indice dans un paquet de donnees (5 octets)
Mrlinkblue 0:9eb40ee5ff41 128 static uint8_t data_received_temp[5];
Mrlinkblue 0:9eb40ee5ff41 129 uint16_t data[4];
Mrlinkblue 0:9eb40ee5ff41 130 //static uint16_t prevData[4];
Mrlinkblue 0:9eb40ee5ff41 131 //static int newData = 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 0:9eb40ee5ff41 139 // on verifie le bit de check et la qualite
Mrlinkblue 0:9eb40ee5ff41 140 if (data[0] &&(data[1]>=8)){
Mrlinkblue 0:9eb40ee5ff41 141 // data -> prevData
Mrlinkblue 6:9af875ef7b30 142 addRadar(data,prevData);
Mrlinkblue 0:9eb40ee5ff41 143 for(int k=0; k<4; k++){
Mrlinkblue 0:9eb40ee5ff41 144 prevData[k] = data[k];
Mrlinkblue 0:9eb40ee5ff41 145 }
Mrlinkblue 0:9eb40ee5ff41 146 newData = 1;
Mrlinkblue 0:9eb40ee5ff41 147 }
Mrlinkblue 6:9af875ef7b30 148
Mrlinkblue 0:9eb40ee5ff41 149 if (data[0] &&(data[1]<8)){
Mrlinkblue 5:73aac5fe9696 150 //addRadar(Radar,data,prevData);
Mrlinkblue 5:73aac5fe9696 151 //for(int k=0; k<3; k++){
Mrlinkblue 5:73aac5fe9696 152 // prevData[k] = data[k];
Mrlinkblue 5:73aac5fe9696 153 //}
Mrlinkblue 0:9eb40ee5ff41 154 }
Mrlinkblue 0:9eb40ee5ff41 155 }
Mrlinkblue 0:9eb40ee5ff41 156 i=(i+1)%5;
Mrlinkblue 0:9eb40ee5ff41 157 }
Mrlinkblue 0:9eb40ee5ff41 158
Mrlinkblue 0:9eb40ee5ff41 159 void setup_lidar(){
Mrlinkblue 0:9eb40ee5ff41 160 // start lidar
Mrlinkblue 0:9eb40ee5ff41 161 wait(0.5);
Mrlinkblue 0:9eb40ee5ff41 162 pwm_lidar.period_us(40);
Mrlinkblue 0:9eb40ee5ff41 163 pwm_lidar.write(0.5);
Mrlinkblue 0:9eb40ee5ff41 164 wait(1);
Mrlinkblue 0:9eb40ee5ff41 165 ld.puts(ldStart);
Mrlinkblue 0:9eb40ee5ff41 166 // on recupere le paquet de depart
Mrlinkblue 0:9eb40ee5ff41 167 recuperer_start(start_data_received);
Mrlinkblue 0:9eb40ee5ff41 168 wait(0.1);
Mrlinkblue 0:9eb40ee5ff41 169 // fonction d interruption
Mrlinkblue 0:9eb40ee5ff41 170 ld.attach(Interrupt_ldRX,Serial::RxIrq);
Mrlinkblue 0:9eb40ee5ff41 171 }