Diff: Communication.cpp

Revision:

0:88706d6abbf7

Child:

1:1ff3fe3679c1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Communication.cpp	Wed Nov 23 15:19:33 2016 +0000
@@ -0,0 +1,177 @@
+#include "mbed.h"
+#include "Communication.h"
+#include "Serial.h"
+#include "BufferedSerial.h"
+#include "math.h"
+#include "LCD_DISCO_F746NG.h"
+#include "Trigo.h"
+
+#define _USE_MATH_DEFINES
+
+
+BufferedSerial lidar(PC_6, PC_7,4096);
+Serial pc(USBTX, USBRX);
+LCD_DISCO_F746NG lcdComm;
+
+void Communication::confUART()
+{
+//  Send command
+    lidar.baud(115200);
+    pc.baud(115200);
+}
+
+
+void Communication::Reset()
+{
+//  Send command
+    lidar.putc(0xA5);
+    lidar.putc(0x40);
+}
+
+void Communication::Stop()
+{
+//  Send command
+    lidar.putc(0xA5);
+    lidar.putc(0x25);
+}
+
+void Communication::Get_Info()
+{
+  short const cardRespReq=7;
+  short const cardRespData=20;
+
+//  Send command
+    lidar.putc(0xA5);
+    lidar.putc(0x50);
+
+  wait_ms(100);
+
+  //Header reading
+  for (int i=0; i<cardRespReq; i++) {
+      pc.putc(lidar.getc());
+  }
+
+  //Data reading
+  for (int i=0; i<cardRespData; i++) {
+      pc.putc(lidar.getc());
+    }
+}
+
+
+void Communication::Get_Health()
+{
+    short const cardRespReq=7;
+    short const cardRespData=3;
+
+      //Send command
+     lidar.putc(0xA5);
+     lidar.putc(0x52);
+
+      wait_ms(100);
+
+      //Header reading
+      for (int i=0; i<cardRespReq; i++) {
+          pc.putc(lidar.getc());
+      }
+
+      //Data reading
+      for (int i=0; i<cardRespData; i++) {
+          pc.putc(lidar.getc());
+        }
+
+}
+
+
+
+
+void Communication::Scan()
+{
+    short const cardResReqInfo=7;
+    struct ScanDataFrame {
+        uint16_t quality;
+        uint16_t angle1,angle2;
+        uint16_t distance1,distance2;
+     };
+
+    struct ScanDataFrame ScanData;
+    uint16_t angle, distance,countClear;
+    float distanceX,distanceY,angRad;
+    uint8_t checkBit;
+    int16_t distanceXLCD,distanceYLCD;
+
+    //Wait for motor stabilization
+    wait_ms(2000);
+
+    //Send command
+                lidar.putc(0xA5);
+                lidar.putc(0x20);
+
+                wait_ms(10);
+
+                for (int i=0; i<cardResReqInfo; i++)
+                        {
+                            pc.putc(lidar.getc());
+                        }
+
+                wait_ms(2000);
+                lcdComm.Clear(LCD_COLOR_BLACK);
+                lcdComm.SetBackColor(LCD_COLOR_GREEN);
+ //             lcdComm.Clear(LCD_COLOR_BLUE);
+//              lcdComm.SetBackColor(LCD_COLOR_BLUE);
+                lcdComm.SetTextColor(LCD_COLOR_WHITE);
+                wait(0.3);
+                
+                countClear=0;
+                while (1)
+                {
+                    ScanData.quality=lidar.getc();
+                    ScanData.angle1=lidar.getc();
+                    ScanData.angle2=lidar.getc();
+                    ScanData.distance1=lidar.getc();
+                    ScanData.distance2=lidar.getc();
+                    checkBit=ScanData.angle1 & 0x1;
+                    if (checkBit==1) {
+                        distance=0;
+                        distance=(uint16_t)(ScanData.distance2<<8 | ScanData.distance1);
+                        distance=distance>>2;
+                        angle=((uint16_t)(ScanData.angle2<<8 | ScanData.angle1)>>1);
+                        angle=angle>>6;
+
+                    if (distance>0)
+                    {
+                        countClear=countClear+1;
+                        angRad=angle*3.14159/180.0;
+                        PolRec(distance,angRad,&distanceY,&distanceX);
+                        if ((abs(distanceX)<6000) && (abs(distanceY)<6000))
+                        {
+                            distanceXLCD=(int16_t)floor(240+((distanceX/6000)*130));
+                            distanceYLCD=(int16_t)floor(130+((distanceY/6000)*130));
+                        }
+
+//                        Debug
+//                        pc.printf("\r");
+//                        pc.printf("Angle: %d Distance: %d X: %f Y: %f",angle,distance,distanceX,distanceY);
+//                        pc.printf("Angle: %d Distance:[mm] %d",angle,distance);
+//                        pc.printf("X: %d Y: %d",distanceX,distanceY);
+//                        pc.printf("Angle: %d Distance: %d X: %d Y: %d",angle,distance,distanceXLCD,distanceYLCD);
+//                        Debug
+//                        
+                         lcdComm.DrawPixel(distanceXLCD,distanceYLCD,LCD_COLOR_GREEN);
+                         wait_ms(1);
+//                        
+                        
+                        if (countClear > 360)
+                        {
+                            lcdComm.Clear(LCD_COLOR_BLACK);
+                            countClear=0;
+                        }
+                        
+                        
+
+                        
+                          
+                    }
+                }
+
+            }
+}