File content as of revision 1:1ff3fe3679c1:

#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.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;
                        }
                        
                    }
                }

            }
}