main.cpp

00001 #include "mbed.h"
00002 #include "LCD_DISCO_F746NG.h"
00003 #include "math.h"
00004 
00005 
00006 //**************************
00007 // Définition des constantes
00008 //**************************
00009 
00010 // L'ensemble des commandes possibles du LIDAR
00011 #define LIDAR_STOP        0x25
00012 #define LIDAR_RESET       0x40
00013 #define LIDAR_SCAN        0x20
00014 #define EXPRESS_SCAN      0x82
00015 #define GET_INFO          0x50
00016 #define GET_HEALTH        0x52
00017 #define GET_SAMPLERATE    0x59
00018 
00019 // Les commandes doivent toutes démarrer par ce caractère
00020 #define START_FLAG        0xA5
00021 
00022 //**************************
00023 // instantiation des objets
00024 //**************************
00025 // Instantiation de l'objet écran LCD
00026 LCD_DISCO_F746NG lcd;
00027 
00028 // Instantiation liaison série ; liaison avec le LIDAR
00029 Serial LIDAR_link(D1,D0);
00030 
00031 // Instatiation d'une sortie PWM sur la pin D3. Son nom est mypwm
00032 PwmOut pwm_Lidar(D6);
00033 
00034 // Instantiation d'un Timer pour l'attente des réponses
00035 Timer horloge ;
00036 
00037 // Instantiation led1
00038 DigitalOut led1(LED1);
00039 
00040 
00041 //*******************
00042 // Variables globales
00043 //*******************
00044 float temps_debut, temps_fin;
00045 
00046 //*************************
00047 // Prototypes des Fonctions
00048 //*************************
00049 bool initialisation();
00050 bool get_health(char* pointeur_etat_sante_lidar);
00051 void radar(void);
00052 bool scan(int8_t* ptr_buffer_donnees_lidar, int16_t Nbre_echantillons);
00053 
00054 // ************************
00055 // Le main
00056 //*************************
00057 int main()
00058 {
00059     bool test;
00060     const int16_t Nbre_echantillons = 200;
00061     int8_t buffer_donnees_lidar[Nbre_echantillons*5] = {0};
00062     int8_t* ptr_buffer_donnees_lidar;
00063 
00064     led1 = 1;
00065 
00066     test = initialisation();
00067 
00068     if (!test) {
00069         lcd.Clear(LCD_COLOR_WHITE);
00070         lcd.SetTextColor(LCD_COLOR_ORANGE);
00071         lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"Probleme etat de sante Lidar", CENTER_MODE);
00072         wait(2);
00073         return 0;
00074     }
00075 
00076     ptr_buffer_donnees_lidar = buffer_donnees_lidar;
00077 
00078     while(1) {
00079         test = scan(ptr_buffer_donnees_lidar, Nbre_echantillons);
00080         wait_ms(200);
00081 
00082     }
00083 }
00084 
00085 //***************************************
00086 //Initialisation : configuration initiale
00087 //***************************************
00088 bool initialisation()
00089 {
00090     bool test ;
00091     char etat_sante_lidar ;
00092     char affichage[3];
00093     char Vidage_buffer_liaison_serie;
00094 
00095     // Initialisation Communication LIDAR
00096     LIDAR_link.baud(115200);
00097     LIDAR_link.format(8,SerialBase::None,1);
00098 
00099     // premier message pour vérifier que tout est OK
00100     lcd.Clear(LCD_COLOR_WHITE);
00101     lcd.SetTextColor(LCD_COLOR_ORANGE);
00102     lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"Initialisation OK", CENTER_MODE);
00103 
00104     // Initialisation PWM
00105     pwm_Lidar.period_us(40);
00106     pwm_Lidar.write(0.7);
00107 
00108     // Vidage buffer liaison série
00109     while (LIDAR_link.readable() == true) {
00110         Vidage_buffer_liaison_serie = LIDAR_link.getc();
00111     }
00112     // test lidar
00113     test = get_health(&etat_sante_lidar);
00114 
00115     if (test == false) {
00116         lcd.Clear(LCD_COLOR_BLUE);
00117         lcd.SetTextColor(LCD_COLOR_ORANGE);
00118         lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"Problemes lidar", CENTER_MODE);
00119         wait(2);
00120 
00121         return false;
00122 
00123     } else {
00124         sprintf(affichage,"%x",etat_sante_lidar);
00125         lcd.Clear(LCD_COLOR_YELLOW);
00126         lcd.SetTextColor(LCD_COLOR_ORANGE);
00127         lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"Lidar OK", CENTER_MODE);
00128         lcd.DisplayStringAt(0, LINE(6), (uint8_t *)affichage, CENTER_MODE);
00129         wait(2);
00130 
00131         radar();
00132 
00133         return true;
00134     }
00135 
00136 } //End Initialisation
00137 
00138 //********************************************
00139 //Fonction lecture de l'état de santé du LIDAR
00140 //********************************************
00141 
00142 bool get_health(char *pointeur_etat_sante_lidar)
00143 {
00144     char response_descriptor[7] = {0,0,0,0,0,0,0};
00145     char response[3];
00146     bool test_time_out;
00147     char ii;
00148     char affichage[3] = {0};
00149     char Vidage_buffer_liaison_serie;
00150 
00151     // Vidage buffer liaison série
00152     while (LIDAR_link.readable() == true) {
00153         Vidage_buffer_liaison_serie = LIDAR_link.getc();
00154     }
00155 
00156     // On attend que la liaison série soit prête en éccriture
00157     while (LIDAR_link.writeable() == false) {
00158     }
00159 
00160     // Envoi de la commande GET_HEALTH
00161     LIDAR_link.putc(START_FLAG);
00162     LIDAR_link.putc(GET_HEALTH);
00163 
00164 
00165     // Attente de la réponse
00166     /*    temps_debut = horloge.read();
00167         test_time_out = true ;
00168 
00169         while ((LIDAR_link.readable() == false) & (test_time_out == true)) {
00170             temps_fin = horloge.read();
00171             test_time_out = (temps_fin - temps_debut) < 2;
00172         }
00173 
00174         if (!test_time_out) {
00175             return false ;
00176         }
00177         */
00178 
00179     // si le délai de réponse n'est pas dépassé
00180 
00181     // acquisition des données
00182     // tout d'abord le descripteur de réponse : 7 octets
00183     for (ii = 0 ; ii <=6 ; ii++) {
00184 
00185         response_descriptor[ii] = LIDAR_link.getc();
00186     }
00187 
00188     // Et on lit 3 octets pour cette requête
00189     for (ii = 0 ; ii <= 2 ; ii++) {
00190         response[ii] = LIDAR_link.getc();
00191     }
00192 
00193  /*   lcd.Clear(LCD_COLOR_WHITE);
00194     lcd.SetTextColor(LCD_COLOR_ORANGE);
00195     for (ii = 0 ; ii <= 6 ; ii++) {
00196         sprintf(affichage,"%x",response_descriptor[ii]);
00197         lcd.DisplayStringAt(0, LINE(ii),(uint8_t*) affichage, LEFT_MODE);
00198     }
00199 
00200     for (ii = 0 ; ii <= 2 ; ii++) {
00201         sprintf(affichage,"%x",response[ii]);
00202         lcd.DisplayStringAt(20, LINE(ii),(uint8_t*) affichage, LEFT_MODE);
00203     }
00204 */
00205     wait_ms(2);
00206 
00207     // l'état de santé du Lidar est donné par le premier octet de la réponse : si 0 pas de problème
00208     *pointeur_etat_sante_lidar = response[0] ;
00209 
00210     return true ;
00211 }
00212 
00213 //********************************************
00214 // Fonction Tracé radar sur l'écran
00215 //********************************************
00216 void radar (void)
00217 {
00218     float angle[13] = {0,30, 60, 90,120,150, 180, 210,240,270, 300, 330, 360};
00219     float distance[13] = {0,1, 2, 3,4,5, 6, 7,8,9, 10, 11, 12};
00220     int8_t jj;
00221     float x, y;
00222 
00223     lcd.Clear(LCD_COLOR_WHITE);
00224     lcd.SetTextColor(LCD_COLOR_BLACK);
00225     lcd.DrawCircle(240,136,136);
00226     lcd.SetTextColor(LCD_COLOR_BLACK);
00227     lcd.DrawCircle(240,136,102);
00228     lcd.SetTextColor(LCD_COLOR_BLACK);
00229     lcd.DrawCircle(240,136,68);
00230     lcd.SetTextColor(LCD_COLOR_BLACK);
00231     lcd.DrawCircle(240,136,34);
00232     lcd.SetTextColor(LCD_COLOR_BLACK);
00233     lcd.DrawLine(104,136,376,136);
00234     lcd.SetTextColor(LCD_COLOR_LIGHTGREEN);
00235     lcd.DrawLine(240,0,240,136);
00236     lcd.SetTextColor(LCD_COLOR_BLACK);
00237     lcd.DrawLine(240,136,240,272);
00238     lcd.SetTextColor(LCD_COLOR_BLACK);
00239     lcd.DrawLine(240,136,358,68);
00240     lcd.SetTextColor(LCD_COLOR_BLACK);
00241     lcd.DrawLine(240,136,308,18);
00242     lcd.SetTextColor(LCD_COLOR_BLACK);
00243     lcd.DrawLine(240,136,172,18);
00244     lcd.SetTextColor(LCD_COLOR_BLACK);
00245     lcd.DrawLine(240,136,122,68);
00246     lcd.SetTextColor(LCD_COLOR_BLACK);
00247     lcd.DrawLine(240,136,172,254);
00248     lcd.SetTextColor(LCD_COLOR_BLACK);
00249     lcd.DrawLine(240,136,122,204);
00250     lcd.SetTextColor(LCD_COLOR_BLACK);
00251     lcd.DrawLine(240,136,308,254);
00252     lcd.SetTextColor(LCD_COLOR_BLACK);
00253     lcd.DrawLine(240,136,358,204);
00254     /**********************************************************************************************************************/
00255 //****************
00256 // test affichage
00257 //****************
00258 
00259     /*    for (jj = 0 ; jj <= 12 ; jj++) {
00260             angle[jj] = angle[jj] * (3.141592 / 180);
00261             x= 240 + distance[jj] * 9 * sin(angle[jj]);
00262             y= 136 - distance[jj] * 9 * cos(angle[jj]);
00263             lcd.SetTextColor(LCD_COLOR_RED);
00264             lcd.FillCircle(x,y,3);
00265         }
00266 
00267         wait(10);
00268     */
00269     return;
00270 }
00271 
00272 //********************************************
00273 // Fonction Scan du LIDAR
00274 //********************************************
00275 
00276 bool scan(int8_t *ptr_buffer_donnees_lidar, int16_t Nbre_echantillons)
00277 {
00278     char response_descriptor[7] = {0};
00279     char response[5];
00280 
00281     char Vidage_buffer_liaison_serie;
00282     char ii, jj;
00283     char data, data1, data2;
00284 
00285     char affichage[8];
00286 
00287     float angle, distance, x, y;
00288     uint16_t decodage_angle ;
00289     uint16_t decodage_distance;
00290     uint8_t  decodage_qualite, S_S;
00291 
00292     bool test_time_out;
00293 
00294 
00295     // On attend que la liaison série soit prête en écriture
00296     while (LIDAR_link.writeable() == false) {
00297     }
00298 
00299 // Envoi de la commande SCAN
00300     LIDAR_link.putc(START_FLAG);
00301     LIDAR_link.putc(LIDAR_SCAN);
00302 
00303     for (ii = 0 ; ii <=6 ; ii++) {
00304         // attente de la réponse
00305         temps_debut = horloge.read();
00306         test_time_out = true ;
00307         while ((LIDAR_link.readable() == false) & (test_time_out == true)) {
00308             temps_fin = horloge.read();
00309             test_time_out = (temps_fin - temps_debut) < 2e-3;
00310         }
00311 
00312         if (test_time_out == false) {
00313             return false;
00314             lcd.Clear(LCD_COLOR_GREEN);
00315         }
00316         response_descriptor[ii] = LIDAR_link.getc();
00317     }
00318 
00319     for (jj = 0 ; jj <= (Nbre_echantillons - 1) ; jj++) {
00320         // Et on lit des paquets de 5 octets pour cette requête
00321         for (ii = 0 ; ii <= 4 ; ii++) {
00322             // attente de la réponse
00323             temps_debut = horloge.read();
00324             test_time_out = true ;
00325             while ((LIDAR_link.readable() == false) & (test_time_out == true)) {
00326                 temps_fin = horloge.read();
00327                 test_time_out = (temps_fin - temps_debut) < 2e-3;
00328             }
00329 
00330             if (test_time_out == false) {
00331                 return false;
00332                 lcd.Clear(LCD_COLOR_GREEN);
00333             }
00334             *(ptr_buffer_donnees_lidar+ ii + 5 * jj) = LIDAR_link.getc();
00335         }
00336     }
00337 
00338     //On arrête l'acquisition une fois le nombre d'échantillons demandés atteints
00339     LIDAR_link.putc(LIDAR_STOP);
00340     wait_ms(2);
00341 
00342     // Vidage buffer liaison série
00343     while (LIDAR_link.readable() == true) {
00344         Vidage_buffer_liaison_serie = LIDAR_link.getc();
00345     }
00346 
00347     /*    lcd.Clear(LCD_COLOR_WHITE);
00348         lcd.SetTextColor(LCD_COLOR_BLACK);
00349 
00350        for (ii = 0 ; ii <= 6 ; ii++) {
00351           sprintf(affichage,"%x",response_descriptor[ii]);
00352          lcd.DisplayStringAt(0, LINE(ii),(uint8_t*) affichage, LEFT_MODE);
00353             }
00354 
00355           for (jj = 0 ; jj <= (Nbre_echantillons - 1) ; jj++) {
00356                 for (ii = 0 ; ii <= 4 ; ii++) {
00357                     data = (*(ptr_buffer_donnees_lidar + ii + 5*jj));
00358                     sprintf(affichage,"%x",data);
00359                     lcd.DisplayStringAt(50+80*jj, LINE(ii),(uint8_t*) affichage, LEFT_MODE);
00360                 }
00361             }
00362      */
00363 
00364     radar();
00365     if ((response_descriptor[0] == 0xA5)&(Nbre_echantillons >=2)) {
00366 
00367         for (jj = 1 ; jj <= (Nbre_echantillons - 1) ; jj++) {
00368 
00369             // test erreur transmission S et _S
00370             data = (*(ptr_buffer_donnees_lidar + 5 * jj));
00371             S_S = data & 0x03;
00372 
00373             // decodage qualité
00374 
00375             decodage_qualite = data & 0xFD ;
00376             decodage_qualite = (decodage_qualite >> 2);
00377 //            sprintf(affichage,"%x",decodage_qualite);
00378 //            lcd.DisplayStringAt(50+80*jj, LINE(6),(uint8_t*) affichage, LEFT_MODE);
00379 
00380             if ((decodage_qualite >= 0x08) && ((S_S == 0x01)|(S_S == 0x02))) {
00381 
00382 
00383                 //decodage angle
00384                 decodage_angle = 0x0000;
00385                 data2 = (*(ptr_buffer_donnees_lidar + 2 + 5 * jj));
00386                 data1 = (*(ptr_buffer_donnees_lidar + 1 + 5 * jj)) ;
00387                 decodage_angle = data2;
00388                 decodage_angle = (((decodage_angle << 8) | data1) >> 1);
00389 
00390                 angle = (decodage_angle/64.0) ; // en degré
00391 //              sprintf(affichage,"%4.0f",angle);
00392 //                lcd.DisplayStringAt(50+80*jj, LINE(7),(uint8_t*) affichage, LEFT_MODE);
00393 
00394 
00395                 //decodage distance
00396                 decodage_distance = 0x0000;
00397                 data2 = (*(ptr_buffer_donnees_lidar + 4 + 5 * jj));
00398                 data1 = (*(ptr_buffer_donnees_lidar + 3 + 5 * jj)) ;
00399                 decodage_distance = data2;
00400                 decodage_distance = ((decodage_distance << 8) | data1);
00401 
00402 
00403                 distance = (decodage_distance/4000.0) ; // en mètres
00404 //                sprintf(affichage,"%4.0f",distance);
00405 //                lcd.DisplayStringAt(50+80*jj, LINE(8),(uint8_t*) affichage, LEFT_MODE);
00406 
00407                 // affichage du point
00408                 angle = angle * (3.141592 / 180);
00409                 x= 240 + distance * 45 * sin(angle);
00410                 y= 136 - distance * 45 * cos(angle);
00411                 lcd.SetTextColor(LCD_COLOR_RED);
00412                 lcd.FillCircle(x,y,1);
00413 
00414 
00415             }// fin if
00416 
00417         } // for jj
00418     } // fin if
00419 
00420     return 1 ;
00421 }