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@1:1ff3fe3679c1, 2016-11-23 (annotated)

Committer:
Carminio
Date:
Wed Nov 23 16:18:07 2016 +0000
Revision:
1:1ff3fe3679c1
Parent:
0:88706d6abbf7
Child:
2:8f71c97fe9d7
Versione con pulsanti da sistemare

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
Carminio 0:88706d6abbf7 12 BufferedSerial lidar(PC_6, PC_7,4096);
Carminio 0:88706d6abbf7 13 Serial pc(USBTX, USBRX);
Carminio 0:88706d6abbf7 14 LCD_DISCO_F746NG lcdComm;
Carminio 0:88706d6abbf7 15
Carminio 0:88706d6abbf7 16 void Communication::confUART()
Carminio 0:88706d6abbf7 17 {
Carminio 0:88706d6abbf7 18 // Send command
Carminio 0:88706d6abbf7 19 lidar.baud(115200);
Carminio 0:88706d6abbf7 20 pc.baud(115200);
Carminio 0:88706d6abbf7 21 }
Carminio 0:88706d6abbf7 22
Carminio 0:88706d6abbf7 23
Carminio 0:88706d6abbf7 24 void Communication::Reset()
Carminio 0:88706d6abbf7 25 {
Carminio 0:88706d6abbf7 26 // Send command
Carminio 0:88706d6abbf7 27 lidar.putc(0xA5);
Carminio 0:88706d6abbf7 28 lidar.putc(0x40);
Carminio 0:88706d6abbf7 29 }
Carminio 0:88706d6abbf7 30
Carminio 0:88706d6abbf7 31 void Communication::Stop()
Carminio 0:88706d6abbf7 32 {
Carminio 0:88706d6abbf7 33 // Send command
Carminio 0:88706d6abbf7 34 lidar.putc(0xA5);
Carminio 0:88706d6abbf7 35 lidar.putc(0x25);
Carminio 0:88706d6abbf7 36 }
Carminio 0:88706d6abbf7 37
Carminio 0:88706d6abbf7 38 void Communication::Get_Info()
Carminio 0:88706d6abbf7 39 {
Carminio 0:88706d6abbf7 40 short const cardRespReq=7;
Carminio 0:88706d6abbf7 41 short const cardRespData=20;
Carminio 0:88706d6abbf7 42
Carminio 0:88706d6abbf7 43 // Send command
Carminio 0:88706d6abbf7 44 lidar.putc(0xA5);
Carminio 0:88706d6abbf7 45 lidar.putc(0x50);
Carminio 0:88706d6abbf7 46
Carminio 0:88706d6abbf7 47 wait_ms(100);
Carminio 0:88706d6abbf7 48
Carminio 0:88706d6abbf7 49 //Header reading
Carminio 0:88706d6abbf7 50 for (int i=0; i<cardRespReq; i++) {
Carminio 0:88706d6abbf7 51 pc.putc(lidar.getc());
Carminio 0:88706d6abbf7 52 }
Carminio 0:88706d6abbf7 53
Carminio 0:88706d6abbf7 54 //Data reading
Carminio 0:88706d6abbf7 55 for (int i=0; i<cardRespData; i++) {
Carminio 0:88706d6abbf7 56 pc.putc(lidar.getc());
Carminio 0:88706d6abbf7 57 }
Carminio 0:88706d6abbf7 58 }
Carminio 0:88706d6abbf7 59
Carminio 0:88706d6abbf7 60
Carminio 0:88706d6abbf7 61 void Communication::Get_Health()
Carminio 0:88706d6abbf7 62 {
Carminio 0:88706d6abbf7 63 short const cardRespReq=7;
Carminio 0:88706d6abbf7 64 short const cardRespData=3;
Carminio 0:88706d6abbf7 65
Carminio 0:88706d6abbf7 66 //Send command
Carminio 0:88706d6abbf7 67 lidar.putc(0xA5);
Carminio 0:88706d6abbf7 68 lidar.putc(0x52);
Carminio 0:88706d6abbf7 69
Carminio 0:88706d6abbf7 70 wait_ms(100);
Carminio 0:88706d6abbf7 71
Carminio 0:88706d6abbf7 72 //Header reading
Carminio 0:88706d6abbf7 73 for (int i=0; i<cardRespReq; i++) {
Carminio 0:88706d6abbf7 74 pc.putc(lidar.getc());
Carminio 0:88706d6abbf7 75 }
Carminio 0:88706d6abbf7 76
Carminio 0:88706d6abbf7 77 //Data reading
Carminio 0:88706d6abbf7 78 for (int i=0; i<cardRespData; i++) {
Carminio 0:88706d6abbf7 79 pc.putc(lidar.getc());
Carminio 0:88706d6abbf7 80 }
Carminio 0:88706d6abbf7 81
Carminio 0:88706d6abbf7 82 }
Carminio 0:88706d6abbf7 83
Carminio 0:88706d6abbf7 84
Carminio 0:88706d6abbf7 85
Carminio 0:88706d6abbf7 86
Carminio 0:88706d6abbf7 87 void Communication::Scan()
Carminio 0:88706d6abbf7 88 {
Carminio 0:88706d6abbf7 89 short const cardResReqInfo=7;
Carminio 0:88706d6abbf7 90 struct ScanDataFrame {
Carminio 0:88706d6abbf7 91 uint16_t quality;
Carminio 0:88706d6abbf7 92 uint16_t angle1,angle2;
Carminio 0:88706d6abbf7 93 uint16_t distance1,distance2;
Carminio 0:88706d6abbf7 94 };
Carminio 0:88706d6abbf7 95
Carminio 0:88706d6abbf7 96 struct ScanDataFrame ScanData;
Carminio 0:88706d6abbf7 97 uint16_t angle, distance,countClear;
Carminio 0:88706d6abbf7 98 float distanceX,distanceY,angRad;
Carminio 0:88706d6abbf7 99 uint8_t checkBit;
Carminio 0:88706d6abbf7 100 int16_t distanceXLCD,distanceYLCD;
Carminio 0:88706d6abbf7 101
Carminio 0:88706d6abbf7 102 //Wait for motor stabilization
Carminio 0:88706d6abbf7 103 wait_ms(2000);
Carminio 0:88706d6abbf7 104
Carminio 0:88706d6abbf7 105 //Send command
Carminio 0:88706d6abbf7 106 lidar.putc(0xA5);
Carminio 0:88706d6abbf7 107 lidar.putc(0x20);
Carminio 0:88706d6abbf7 108
Carminio 0:88706d6abbf7 109 wait_ms(10);
Carminio 0:88706d6abbf7 110
Carminio 0:88706d6abbf7 111 for (int i=0; i<cardResReqInfo; i++)
Carminio 0:88706d6abbf7 112 {
Carminio 0:88706d6abbf7 113 pc.putc(lidar.getc());
Carminio 0:88706d6abbf7 114 }
Carminio 0:88706d6abbf7 115
Carminio 0:88706d6abbf7 116 wait_ms(2000);
Carminio 1:1ff3fe3679c1 117 //lcdComm.Clear(LCD_COLOR_BLACK);
Carminio 0:88706d6abbf7 118 lcdComm.SetBackColor(LCD_COLOR_GREEN);
Carminio 1:1ff3fe3679c1 119 // lcdComm.SetTextColor(LCD_COLOR_WHITE);
Carminio 0:88706d6abbf7 120 wait(0.3);
Carminio 0:88706d6abbf7 121
Carminio 0:88706d6abbf7 122 countClear=0;
Carminio 0:88706d6abbf7 123 while (1)
Carminio 0:88706d6abbf7 124 {
Carminio 0:88706d6abbf7 125 ScanData.quality=lidar.getc();
Carminio 0:88706d6abbf7 126 ScanData.angle1=lidar.getc();
Carminio 0:88706d6abbf7 127 ScanData.angle2=lidar.getc();
Carminio 0:88706d6abbf7 128 ScanData.distance1=lidar.getc();
Carminio 0:88706d6abbf7 129 ScanData.distance2=lidar.getc();
Carminio 0:88706d6abbf7 130 checkBit=ScanData.angle1 & 0x1;
Carminio 0:88706d6abbf7 131 if (checkBit==1) {
Carminio 0:88706d6abbf7 132 distance=0;
Carminio 0:88706d6abbf7 133 distance=(uint16_t)(ScanData.distance2<<8 | ScanData.distance1);
Carminio 0:88706d6abbf7 134 distance=distance>>2;
Carminio 0:88706d6abbf7 135 angle=((uint16_t)(ScanData.angle2<<8 | ScanData.angle1)>>1);
Carminio 0:88706d6abbf7 136 angle=angle>>6;
Carminio 0:88706d6abbf7 137
Carminio 0:88706d6abbf7 138 if (distance>0)
Carminio 0:88706d6abbf7 139 {
Carminio 0:88706d6abbf7 140 countClear=countClear+1;
Carminio 0:88706d6abbf7 141 angRad=angle*3.14159/180.0;
Carminio 0:88706d6abbf7 142 PolRec(distance,angRad,&distanceY,&distanceX);
Carminio 0:88706d6abbf7 143 if ((abs(distanceX)<6000) && (abs(distanceY)<6000))
Carminio 0:88706d6abbf7 144 {
Carminio 0:88706d6abbf7 145 distanceXLCD=(int16_t)floor(240+((distanceX/6000)*130));
Carminio 0:88706d6abbf7 146 distanceYLCD=(int16_t)floor(130+((distanceY/6000)*130));
Carminio 0:88706d6abbf7 147 }
Carminio 0:88706d6abbf7 148
Carminio 0:88706d6abbf7 149 // Debug
Carminio 0:88706d6abbf7 150 // pc.printf("\r");
Carminio 0:88706d6abbf7 151 // pc.printf("Angle: %d Distance: %d X: %f Y: %f",angle,distance,distanceX,distanceY);
Carminio 0:88706d6abbf7 152 // pc.printf("Angle: %d Distance:[mm] %d",angle,distance);
Carminio 0:88706d6abbf7 153 // pc.printf("X: %d Y: %d",distanceX,distanceY);
Carminio 0:88706d6abbf7 154 // pc.printf("Angle: %d Distance: %d X: %d Y: %d",angle,distance,distanceXLCD,distanceYLCD);
Carminio 0:88706d6abbf7 155 // Debug
Carminio 0:88706d6abbf7 156 //
Carminio 0:88706d6abbf7 157 lcdComm.DrawPixel(distanceXLCD,distanceYLCD,LCD_COLOR_GREEN);
Carminio 0:88706d6abbf7 158 wait_ms(1);
Carminio 1:1ff3fe3679c1 159
Carminio 0:88706d6abbf7 160
Carminio 0:88706d6abbf7 161 if (countClear > 360)
Carminio 0:88706d6abbf7 162 {
Carminio 0:88706d6abbf7 163 lcdComm.Clear(LCD_COLOR_BLACK);
Carminio 0:88706d6abbf7 164 countClear=0;
Carminio 0:88706d6abbf7 165 }
Carminio 0:88706d6abbf7 166
Carminio 0:88706d6abbf7 167 }
Carminio 0:88706d6abbf7 168 }
Carminio 0:88706d6abbf7 169
Carminio 0:88706d6abbf7 170 }
Carminio 0:88706d6abbf7 171 }