Communication.cpp

00001 #include "mbed.h"
00002 #include "Communication.h"
00003 #include "Serial.h"
00004 #include "BufferedSerial.h"
00005 #include "math.h"
00006 #include "LCD_DISCO_F746NG.h"
00007 #include "Trigo.h"
00008 
00009 #define _USE_MATH_DEFINES
00010 
00011 
00012 BufferedSerial lidar(PC_6, PC_7,4096);         // TX RX BAUD
00013 Serial pc(USBTX, USBRX);
00014 LCD_DISCO_F746NG lcdComm;  //Init LCD
00015 
00016 void Communication::confUART()
00017 {
00018      //ENVOIE REQUETE
00019     lidar.baud(115200);
00020     pc.baud(115200);
00021     
00022     
00023     
00024 }
00025 
00026 
00027 void Communication::Reset()
00028 {
00029      //ENVOIE REQUETE
00030     lidar.putc(0xA5);
00031     lidar.putc(0x40);
00032     wait_ms(2);
00033 }
00034 
00035 void Communication::Stop()
00036 {
00037      //ENVOIE REQUETE
00038     lidar.putc(0xA5);
00039     lidar.putc(0x25);
00040     
00041      wait_ms(1);
00042 }
00043 
00044 void Communication::Get_Info()
00045 {
00046   short const cardRespReq=7;
00047   short const cardRespData=20;
00048 
00049      //ENVOIE REQUETE
00050     lidar.putc(0xA5);
00051     lidar.putc(0x50);
00052 
00053   wait_ms(100);
00054 
00055   //LECTURE DE L'ENTETE
00056   for (int i=0; i<cardRespReq; i++) {
00057       pc.putc(lidar.getc());
00058   }
00059 
00060   
00061       //LECTURE DONNEE
00062   for (int i=0; i<cardRespData; i++) {
00063       pc.putc(lidar.getc());
00064     }
00065 }
00066 
00067 
00068 void Communication::Get_Health()
00069 {
00070     short const cardRespReq=7;
00071     short const cardRespData=3;
00072 
00073      //ENVOIE REQUETE
00074      lidar.putc(0xA5);    
00075      lidar.putc(0x52);
00076 
00077       wait_ms(100);
00078 
00079       for (int i=0; i<cardRespReq; i++) {
00080           pc.putc(lidar.getc());
00081       }
00082 
00083       //LECTURE DONNEES
00084       for (int i=0; i<cardRespData; i++) {
00085           pc.putc(lidar.getc());
00086         }
00087 
00088 }
00089 
00090 
00091 
00092 
00093 void Communication::Scan()
00094 {
00095     short const cardResReqInfo=7;
00096     struct ScanDataFrame {
00097         uint16_t quality;
00098         uint16_t angle1,angle2;
00099         uint16_t distance1,distance2;
00100      };
00101 
00102     struct ScanDataFrame ScanData;
00103     uint16_t angle, distance,countClear;
00104     float distanceX,distanceY,angRad;
00105     uint8_t checkBit;
00106     int16_t distanceXLCD,distanceYLCD;
00107 
00108    
00109 //              ENVOIE REQUETE
00110                 lidar.putc(0xA5);
00111                 lidar.putc(0x20);
00112 
00113                 wait_ms(10);
00114 
00115                 for (int i=0; i<cardResReqInfo; i++)
00116                         {
00117                             pc.putc(lidar.getc());
00118                         }
00119 //              ATTENTE STABILISATION
00120                 wait_ms(1000);
00121                 lcdComm.SetBackColor(LCD_COLOR_GREEN);
00122                 wait(0.3);
00123                 
00124                 countClear=0;
00125                 while (1)
00126                 {
00127                     
00128                    //LECTURE DONNEEs
00129                     ScanData.quality=lidar.getc();
00130                     ScanData.angle1=lidar.getc();
00131                     ScanData.angle2=lidar.getc();
00132                     ScanData.distance1=lidar.getc();
00133                     ScanData.distance2=lidar.getc();
00134                     checkBit=ScanData.angle1 & 0x1;
00135                     
00136                     
00137                     
00138                     // RESTRUCTURATION DES DONNEES
00139                     if (checkBit==1) {
00140                         distance=0;
00141                         distance=(uint16_t)(ScanData.distance2<<8 | ScanData.distance1);
00142                         distance=distance>>2;
00143                         angle=((uint16_t)(ScanData.angle2<<8 | ScanData.angle1)>>1);
00144                         angle=angle>>6;
00145 
00146                     if (distance>0)
00147                     {
00148                         countClear=countClear+1;
00149                         angRad=angle*3.14159/180.0;
00150                         PolRec(distance,angRad,&distanceY,&distanceX);
00151                         if ((abs(distanceX)<6000) && (abs(distanceY)<6000))
00152                         {
00153                             
00154                             // TENTATIVE 
00155                             distanceXLCD=(int16_t)floor(240+((distanceX/6000)*130));
00156                             distanceYLCD=(int16_t)floor(130+((distanceY/6000)*130));
00157                         }
00158 
00159 
00160 
00161 
00162                         // DEBUGAGE SUR PC 
00163                         pc.printf("\r");
00164                         pc.printf("Angle: %d Distance: %d X: %f Y: %f",angle,distance,distanceX,distanceY);
00165                         pc.printf("Angle: %d Distance:[mm] %d",angle,distance);
00166                         pc.printf("X: %d Y: %d",distanceX,distanceY);
00167                         pc.printf("Angle: %d Distance: %d X: %d Y: %d",angle,distance,distanceXLCD,distanceYLCD);
00168                     
00169                         // AFFICHE UN POINT SUR LCD EN FONCTION DE DX ET DY
00170                          lcdComm.DrawPixel(distanceXLCD,distanceYLCD,LCD_COLOR_WHITE);
00171                          wait_ms(1);
00172                         
00173                         
00174                         if (countClear > 360)
00175                         {
00176                             lcdComm.Clear(LCD_COLOR_BLACK);
00177                             countClear=0;
00178                         }
00179                         
00180                     }
00181                 }
00182 
00183             }
00184 }