ST_DuckieTeam
Lidar
Dependencies: TS_DISCO_F746NG mbed BufferedSerial LCD_DISCO_F746NG mbed-rtos Trigo BSP_DISCO_F746NG
Revision 0:88706d6abbf7, committed 2016-11-23
- Comitter:
- Carminio
- Date:
- Wed Nov 23 15:19:33 2016 +0000
- Child:
- 1:1ff3fe3679c1
- Commit message:
- Ver OK senza pulsanti e TFT
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BSP_DISCO_F746NG.lib Wed Nov 23 15:19:33 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/ST/code/BSP_DISCO_F746NG/#fe313c53cdb5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BufferedSerial.lib Wed Nov 23 15:19:33 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/sam_grove/code/BufferedSerial/#a0d37088b405
--- /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;
+ }
+
+
+
+
+
+ }
+ }
+
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Communication.h Wed Nov 23 15:19:33 2016 +0000
@@ -0,0 +1,32 @@
+#include "mbed.h"
+#include "Serial.h"
+#include "BufferedSerial.h"
+
+
+class Communication// : public BufferedSerial
+
+{
+ public:
+ //Communication(PinName tx, PinName rx, uint32_t sizeBuffer) : BufferedSerial(tx, rx) {
+ Communication() {
+ }
+
+ public:
+ void confUART();
+
+ public:
+ void Reset();
+
+ public:
+ void Stop();
+
+ public:
+ void Get_Info();
+
+ public:
+ void Get_Health();
+
+ public:
+ void Scan();
+
+};
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LCD_DISCO_F746NG.lib Wed Nov 23 15:19:33 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/ST/code/LCD_DISCO_F746NG/#d44525b1de98
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Main.cpp Wed Nov 23 15:19:33 2016 +0000
@@ -0,0 +1,52 @@
+#include "mbed.h"
+#include "Serial.h"
+#include "BufferedSerial.h"
+#include "Communication.h"
+#include "Motor.h"
+#include <stdlib.h>
+#include <stdio.h>
+#include "LCD_DISCO_F746NG.h"
+
+LCD_DISCO_F746NG lcd;
+
+
+//Dichiarazione oggetti
+DigitalIn myStartButton(USER_BUTTON);
+Motor lid_Motor(PWM_OUT);
+Communication SendCommands;
+
+int main() {
+// Initialize Display
+ lcd.Clear(LCD_COLOR_BLACK);
+ lcd.SetBackColor(LCD_COLOR_BLACK);
+ lcd.SetTextColor(LCD_COLOR_GREEN);
+ wait(0.3);
+ lcd.SetFont(&Font24);
+ lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"SLAMTEC LIDAR A2", CENTER_MODE);
+
+ if (myStartButton == 1) {
+
+// Initialize UART
+ SendCommands.confUART();
+
+ // Perform a device reset
+// SendCommands.Reset();
+
+ // Send Get_Health command
+// SendCommands.Get_Health();
+
+ // Send Get_Info command
+// SendCommands.Get_Info();
+
+ // Start motor (PWM: Frequency 25KHz, Duty cycle 60%)
+ lid_Motor.StartMotor_Scan();
+
+// Start scan
+ SendCommands.Scan();
+
+// SendCommands.Stop();
+// wait_ms(100);
+// lid_Motor.StopMotor_Scan();
+ }
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.cpp Wed Nov 23 15:19:33 2016 +0000
@@ -0,0 +1,23 @@
+#include "mbed.h"
+#include "Motor.h"
+
+
+void Motor::StartMotor_Scan()
+{
+ int period=40; //Frequency 25KHz
+ int pulsewidth=24; //Duty cycle 60%
+
+ period_us(period);
+ pulsewidth_us(pulsewidth);
+}
+
+
+void Motor::StopMotor_Scan()
+{
+ int period=40; //Frequency 0Hz
+ int pulsewidth=0; //Duty cycle 0%
+
+ period_us(period);
+ pulsewidth_us(pulsewidth);
+
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.h Wed Nov 23 15:19:33 2016 +0000
@@ -0,0 +1,16 @@
+#include "mbed.h"
+
+
+class Motor : public PwmOut
+{
+ public:
+
+ Motor(PinName pin) : PwmOut(pin) {
+ }
+
+ public:
+ void StartMotor_Scan();
+
+ public:
+ void StopMotor_Scan();
+};
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Trigo.lib Wed Nov 23 15:19:33 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/cromda/code/Trigo/#690d6de2df16
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Nov 23 15:19:33 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/0ab6a29f35bf \ No newline at end of file