Kheirddine Zaillel / Mbed 2 deprecated ZAILLEL_Interface

RPLIDAR A1M8 LCD

Dependencies:   TS_DISCO_F746NG mbed BufferedSerial LCD_DISCO_F746NG mbed-rtos Trigo BSP_DISCO_F746NG

Communication.cpp@8:7788cc96e8ad, 2021-06-22 (annotated)

Committer:
KheirddineZ
Date:
Tue Jun 22 13:22:01 2021 +0000
Revision:
8:7788cc96e8ad
Parent:
5:734551a9f4e7 
RPLIDAR A1M8

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Carminio 0:88706d6abbf7 1 #include "mbed.h"
Carminio 0:88706d6abbf7 2 #include "Communication.h"
Carminio 0:88706d6abbf7 3 #include "Serial.h"
Carminio 0:88706d6abbf7 4 #include "BufferedSerial.h"
Carminio 0:88706d6abbf7 5 #include "math.h"
Carminio 0:88706d6abbf7 6 #include "LCD_DISCO_F746NG.h"
Carminio 0:88706d6abbf7 7 #include "Trigo.h"
Carminio 0:88706d6abbf7 8
Carminio 0:88706d6abbf7 9 #define _USE_MATH_DEFINES
Carminio 0:88706d6abbf7 10
Carminio 0:88706d6abbf7 11
KheirddineZ 8:7788cc96e8ad 12 BufferedSerial lidar(PC_6, PC_7,4096); // TX RX BAUD
Carminio 0:88706d6abbf7 13 Serial pc(USBTX, USBRX);
KheirddineZ 8:7788cc96e8ad 14 LCD_DISCO_F746NG lcdComm; //Init LCD
Carminio 0:88706d6abbf7 15
Carminio 0:88706d6abbf7 16 void Communication::confUART()
Carminio 0:88706d6abbf7 17 {
KheirddineZ 8:7788cc96e8ad 18 //ENVOIE REQUETE
Carminio 0:88706d6abbf7 19 lidar.baud(115200);
Carminio 0:88706d6abbf7 20 pc.baud(115200);
KheirddineZ 8:7788cc96e8ad 21
KheirddineZ 8:7788cc96e8ad 22
KheirddineZ 8:7788cc96e8ad 23
Carminio 0:88706d6abbf7 24 }
Carminio 0:88706d6abbf7 25
Carminio 0:88706d6abbf7 26
Carminio 0:88706d6abbf7 27 void Communication::Reset()
Carminio 0:88706d6abbf7 28 {
KheirddineZ 8:7788cc96e8ad 29 //ENVOIE REQUETE
Carminio 0:88706d6abbf7 30 lidar.putc(0xA5);
Carminio 0:88706d6abbf7 31 lidar.putc(0x40);
KheirddineZ 8:7788cc96e8ad 32 wait_ms(2);
Carminio 0:88706d6abbf7 33 }
Carminio 0:88706d6abbf7 34
Carminio 0:88706d6abbf7 35 void Communication::Stop()
Carminio 0:88706d6abbf7 36 {
KheirddineZ 8:7788cc96e8ad 37 //ENVOIE REQUETE
Carminio 0:88706d6abbf7 38 lidar.putc(0xA5);
Carminio 0:88706d6abbf7 39 lidar.putc(0x25);
KheirddineZ 8:7788cc96e8ad 40
KheirddineZ 8:7788cc96e8ad 41 wait_ms(1);
Carminio 0:88706d6abbf7 42 }
Carminio 0:88706d6abbf7 43
Carminio 0:88706d6abbf7 44 void Communication::Get_Info()
Carminio 0:88706d6abbf7 45 {
Carminio 0:88706d6abbf7 46 short const cardRespReq=7;
Carminio 0:88706d6abbf7 47 short const cardRespData=20;
Carminio 0:88706d6abbf7 48
KheirddineZ 8:7788cc96e8ad 49 //ENVOIE REQUETE
Carminio 0:88706d6abbf7 50 lidar.putc(0xA5);
Carminio 0:88706d6abbf7 51 lidar.putc(0x50);
Carminio 0:88706d6abbf7 52
Carminio 0:88706d6abbf7 53 wait_ms(100);
Carminio 0:88706d6abbf7 54
KheirddineZ 8:7788cc96e8ad 55 //LECTURE DE L'ENTETE
Carminio 0:88706d6abbf7 56 for (int i=0; i<cardRespReq; i++) {
Carminio 0:88706d6abbf7 57 pc.putc(lidar.getc());
Carminio 0:88706d6abbf7 58 }
Carminio 0:88706d6abbf7 59
KheirddineZ 8:7788cc96e8ad 60
KheirddineZ 8:7788cc96e8ad 61 //LECTURE DONNEE
Carminio 0:88706d6abbf7 62 for (int i=0; i<cardRespData; i++) {
Carminio 0:88706d6abbf7 63 pc.putc(lidar.getc());
Carminio 0:88706d6abbf7 64 }
Carminio 0:88706d6abbf7 65 }
Carminio 0:88706d6abbf7 66
Carminio 0:88706d6abbf7 67
Carminio 0:88706d6abbf7 68 void Communication::Get_Health()
Carminio 0:88706d6abbf7 69 {
Carminio 0:88706d6abbf7 70 short const cardRespReq=7;
Carminio 0:88706d6abbf7 71 short const cardRespData=3;
Carminio 0:88706d6abbf7 72
KheirddineZ 8:7788cc96e8ad 73 //ENVOIE REQUETE
KheirddineZ 8:7788cc96e8ad 74 lidar.putc(0xA5);
Carminio 0:88706d6abbf7 75 lidar.putc(0x52);
Carminio 0:88706d6abbf7 76
Carminio 0:88706d6abbf7 77 wait_ms(100);
Carminio 0:88706d6abbf7 78
Carminio 0:88706d6abbf7 79 for (int i=0; i<cardRespReq; i++) {
Carminio 0:88706d6abbf7 80 pc.putc(lidar.getc());
Carminio 0:88706d6abbf7 81 }
Carminio 0:88706d6abbf7 82
KheirddineZ 8:7788cc96e8ad 83 //LECTURE DONNEES
Carminio 0:88706d6abbf7 84 for (int i=0; i<cardRespData; i++) {
Carminio 0:88706d6abbf7 85 pc.putc(lidar.getc());
Carminio 0:88706d6abbf7 86 }
Carminio 0:88706d6abbf7 87
Carminio 0:88706d6abbf7 88 }
Carminio 0:88706d6abbf7 89
Carminio 0:88706d6abbf7 90
Carminio 0:88706d6abbf7 91
Carminio 0:88706d6abbf7 92
Carminio 0:88706d6abbf7 93 void Communication::Scan()
Carminio 0:88706d6abbf7 94 {
Carminio 0:88706d6abbf7 95 short const cardResReqInfo=7;
Carminio 0:88706d6abbf7 96 struct ScanDataFrame {
Carminio 0:88706d6abbf7 97 uint16_t quality;
Carminio 0:88706d6abbf7 98 uint16_t angle1,angle2;
Carminio 0:88706d6abbf7 99 uint16_t distance1,distance2;
Carminio 0:88706d6abbf7 100 };
Carminio 0:88706d6abbf7 101
Carminio 0:88706d6abbf7 102 struct ScanDataFrame ScanData;
Carminio 0:88706d6abbf7 103 uint16_t angle, distance,countClear;
Carminio 0:88706d6abbf7 104 float distanceX,distanceY,angRad;
Carminio 0:88706d6abbf7 105 uint8_t checkBit;
Carminio 0:88706d6abbf7 106 int16_t distanceXLCD,distanceYLCD;
Carminio 0:88706d6abbf7 107
Carminio 5:734551a9f4e7 108
KheirddineZ 8:7788cc96e8ad 109 // ENVOIE REQUETE
Carminio 0:88706d6abbf7 110 lidar.putc(0xA5);
Carminio 0:88706d6abbf7 111 lidar.putc(0x20);
Carminio 0:88706d6abbf7 112
Carminio 0:88706d6abbf7 113 wait_ms(10);
Carminio 0:88706d6abbf7 114
Carminio 0:88706d6abbf7 115 for (int i=0; i<cardResReqInfo; i++)
Carminio 0:88706d6abbf7 116 {
Carminio 0:88706d6abbf7 117 pc.putc(lidar.getc());
Carminio 0:88706d6abbf7 118 }
KheirddineZ 8:7788cc96e8ad 119 // ATTENTE STABILISATION
Carminio 2:8f71c97fe9d7 120 wait_ms(1000);
Carminio 0:88706d6abbf7 121 lcdComm.SetBackColor(LCD_COLOR_GREEN);
Carminio 0:88706d6abbf7 122 wait(0.3);
Carminio 0:88706d6abbf7 123
Carminio 0:88706d6abbf7 124 countClear=0;
Carminio 0:88706d6abbf7 125 while (1)
Carminio 0:88706d6abbf7 126 {
KheirddineZ 8:7788cc96e8ad 127
KheirddineZ 8:7788cc96e8ad 128 //LECTURE DONNEEs
Carminio 0:88706d6abbf7 129 ScanData.quality=lidar.getc();
Carminio 0:88706d6abbf7 130 ScanData.angle1=lidar.getc();
Carminio 0:88706d6abbf7 131 ScanData.angle2=lidar.getc();
Carminio 0:88706d6abbf7 132 ScanData.distance1=lidar.getc();
Carminio 0:88706d6abbf7 133 ScanData.distance2=lidar.getc();
Carminio 0:88706d6abbf7 134 checkBit=ScanData.angle1 & 0x1;
KheirddineZ 8:7788cc96e8ad 135
KheirddineZ 8:7788cc96e8ad 136
KheirddineZ 8:7788cc96e8ad 137
KheirddineZ 8:7788cc96e8ad 138 // RESTRUCTURATION DES DONNEES
Carminio 0:88706d6abbf7 139 if (checkBit==1) {
Carminio 0:88706d6abbf7 140 distance=0;
Carminio 0:88706d6abbf7 141 distance=(uint16_t)(ScanData.distance2<<8 | ScanData.distance1);
Carminio 0:88706d6abbf7 142 distance=distance>>2;
Carminio 0:88706d6abbf7 143 angle=((uint16_t)(ScanData.angle2<<8 | ScanData.angle1)>>1);
Carminio 0:88706d6abbf7 144 angle=angle>>6;
Carminio 0:88706d6abbf7 145
Carminio 0:88706d6abbf7 146 if (distance>0)
Carminio 0:88706d6abbf7 147 {
Carminio 0:88706d6abbf7 148 countClear=countClear+1;
Carminio 0:88706d6abbf7 149 angRad=angle*3.14159/180.0;
Carminio 0:88706d6abbf7 150 PolRec(distance,angRad,&distanceY,&distanceX);
Carminio 0:88706d6abbf7 151 if ((abs(distanceX)<6000) && (abs(distanceY)<6000))
Carminio 0:88706d6abbf7 152 {
KheirddineZ 8:7788cc96e8ad 153
KheirddineZ 8:7788cc96e8ad 154 // TENTATIVE
Carminio 0:88706d6abbf7 155 distanceXLCD=(int16_t)floor(240+((distanceX/6000)*130));
Carminio 0:88706d6abbf7 156 distanceYLCD=(int16_t)floor(130+((distanceY/6000)*130));
Carminio 0:88706d6abbf7 157 }
Carminio 0:88706d6abbf7 158
KheirddineZ 8:7788cc96e8ad 159
KheirddineZ 8:7788cc96e8ad 160
KheirddineZ 8:7788cc96e8ad 161
KheirddineZ 8:7788cc96e8ad 162 // DEBUGAGE SUR PC
KheirddineZ 8:7788cc96e8ad 163 pc.printf("\r");
KheirddineZ 8:7788cc96e8ad 164 pc.printf("Angle: %d Distance: %d X: %f Y: %f",angle,distance,distanceX,distanceY);
KheirddineZ 8:7788cc96e8ad 165 pc.printf("Angle: %d Distance:[mm] %d",angle,distance);
KheirddineZ 8:7788cc96e8ad 166 pc.printf("X: %d Y: %d",distanceX,distanceY);
KheirddineZ 8:7788cc96e8ad 167 pc.printf("Angle: %d Distance: %d X: %d Y: %d",angle,distance,distanceXLCD,distanceYLCD);
KheirddineZ 8:7788cc96e8ad 168
KheirddineZ 8:7788cc96e8ad 169 // AFFICHE UN POINT SUR LCD EN FONCTION DE DX ET DY
KheirddineZ 8:7788cc96e8ad 170 lcdComm.DrawPixel(distanceXLCD,distanceYLCD,LCD_COLOR_WHITE);
Carminio 0:88706d6abbf7 171 wait_ms(1);
Carminio 1:1ff3fe3679c1 172
Carminio 0:88706d6abbf7 173
Carminio 0:88706d6abbf7 174 if (countClear > 360)
Carminio 0:88706d6abbf7 175 {
Carminio 0:88706d6abbf7 176 lcdComm.Clear(LCD_COLOR_BLACK);
Carminio 0:88706d6abbf7 177 countClear=0;
Carminio 0:88706d6abbf7 178 }
Carminio 0:88706d6abbf7 179
Carminio 0:88706d6abbf7 180 }
Carminio 0:88706d6abbf7 181 }
Carminio 0:88706d6abbf7 182
Carminio 0:88706d6abbf7 183 }
KheirddineZ 8:7788cc96e8ad 184 }