Diff: main.cpp

Revision:

17:e1a5d25218de

Parent:

14:0e6b26c1a7c5

Child:

18:7ed985584ec7

--- a/main.cpp	Thu Apr 25 05:00:18 2019 +0000
+++ b/main.cpp	Thu Apr 25 05:56:18 2019 +0000
@@ -1,19 +1,43 @@
 #include "globals.h"
 #include "DataDistributor.h"
 #include "RobotController.h"
+#include "XNucleo53L0A1.h"
 
 Serial pc(USBTX, USBRX);
 BusOut led(LED1, LED2, LED3, LED4);
 DigitalIn pb(p23);
+Thread lidarThread;
 
 int* obstacles = new int[360];
 int trajectoryLength = 0;
 int* trajectory = new int[0];
+uint32_t lidarDistance = 0.0;
+bool useImu = false;
+
+void getLidarDistance() {
+    DevI2C* lidarDevice = new DevI2C(p28, p27);
+    XNucleo53L0A1* lidarBoard = XNucleo53L0A1::instance(lidarDevice, A2, D8, D2);
+    DigitalOut _lidarShdn(p26);
+    _lidarShdn = 0;
+    wait(0.1);
+    _lidarShdn = 1;
+    wait(0.1);
+    int status = lidarBoard->init_board();
+    while (status) {
+        status = lidarBoard->init_board();
+    }
+    while(1) {
+        if (!useImu) {
+            lidarBoard->sensor_centre->get_distance(&lidarDistance);
+        }
+    }
+}
 
 int main() {
     delete []trajectory;
     DataDistributor dataDistributor;
-    RobotController robotController(p21, p15, p16, p22, p18, p19, p13, p14, p26, p28, p27);
+    RobotController robotController(p21, p15, p16, p22, p18, p19, p13, p14, p28, p27);
+    lidarThread.start(getLidarDistance);
 
     while(1) {
         robotController.detectObstacles();