Diff: RobotController.cpp

Revision:

17:e1a5d25218de

Parent:

16:5c38e24db603

Child:

18:7ed985584ec7

--- a/RobotController.cpp	Thu Apr 25 05:00:18 2019 +0000
+++ b/RobotController.cpp	Thu Apr 25 05:56:18 2019 +0000
@@ -3,22 +3,11 @@
 RobotController::RobotController(PinName leftWheelPwm, PinName leftWheelFwd,
     PinName leftWheelRev, PinName rightWheelPwm, PinName rightWheelFwd,
     PinName rightWheelRev, PinName leftEncoder, PinName rightEncoder,
-    PinName lidarShdn, PinName sda, PinName scl) :
+    PinName imuSda, PinName imuScl) :
     leftWheel(leftWheelPwm, leftWheelFwd, leftWheelRev),
     rightWheel(rightWheelPwm, rightWheelFwd, rightWheelRev),
-    _leftEncoder(leftEncoder), _rightEncoder(rightEncoder), _lidarShdn(lidarShdn),
-    imu(sda, scl, 0xD6, 0x3C) {
-        lidarDevice = new DevI2C(sda, scl);
-        lidarBoard = XNucleo53L0A1::instance(lidarDevice, A2, D8, D2);
-        _lidarShdn = 0;
-        wait(0.1);
-        _lidarShdn = 1;
-        wait(0.1);
-        int status = lidarBoard->init_board();
-        while(status) {
-            status = lidarBoard->init_board();
-        }
-        lidarDistance = 0;
+    _leftEncoder(leftEncoder), _rightEncoder(rightEncoder),
+    imu(imuSda, imuScl, 0xD6, 0x3C) {
         imu.begin();
         imu.calibrate();
         w1 = 0.0;
@@ -46,7 +35,6 @@
         while((_leftEncoder.read() < 1) && (_rightEncoder.read() < 1));
         leftWheel.speed(0);
         rightWheel.speed(0);
-        lidarBoard->sensor_centre->get_distance(&lidarDistance);
         obstacles[i] = (int)(lidarDistance*MMTOIN);
     }
     led = 0;
@@ -70,6 +58,7 @@
         } else if (trajectory[i] < 360) {
             angle = angle*ROTERRIII;
         }
+        useImu = true;
         t.start();
         leftWheel.speed(0.2);
         rightWheel.speed(-0.2);
@@ -85,6 +74,7 @@
         leftWheel.speed(0);
         rightWheel.speed(0);
         t.stop();
+        useImu = false;
         _leftEncoder.reset();
         _rightEncoder.reset();
         int distance = (int)(trajectory[i + 1]*COUNTPERIN);