Diff: RobotController.cpp

Revision:

14:0e6b26c1a7c5

Parent:

11:531208aca075

Child:

16:5c38e24db603

--- a/RobotController.cpp	Tue Apr 23 20:00:04 2019 +0000
+++ b/RobotController.cpp	Thu Apr 25 03:21:17 2019 +0000
@@ -3,11 +3,22 @@
 RobotController::RobotController(PinName leftWheelPwm, PinName leftWheelFwd,
     PinName leftWheelRev, PinName rightWheelPwm, PinName rightWheelFwd,
     PinName rightWheelRev, PinName leftEncoder, PinName rightEncoder,
-    PinName imuSda, PinName imuScl) :
+    PinName lidarShdn, PinName sda, PinName scl) :
     leftWheel(leftWheelPwm, leftWheelFwd, leftWheelRev),
     rightWheel(rightWheelPwm, rightWheelFwd, rightWheelRev),
-    leftEncoder(leftEncoder), rightEncoder(rightEncoder),
-    imu(imuSda, imuScl, 0xD6, 0x3C) {
+    _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;
         imu.begin();
         imu.calibrate();
         w1 = 0.0;
@@ -28,24 +39,15 @@
 void RobotController::detectObstacles() {
     led = 0b0001;
     for (int i = 0; i < 360; i++) {
-        leftEncoder.reset();
-        rightEncoder.reset();
-        t.start();
-        leftWheel.speed(0.2);
-        rightWheel.speed(-0.2);
-//        while((leftEncoder.read() < COUNTPERDEG) && (rightEncoder.read() < COUNTPERDEG));
-        while((((w2+w1)/2.0)*(t2-t1)) < 1.0) {
-            while(!imu.gyroAvailable());
-            imu.readGyro();
-            w1 = w2;
-            w2 = imu.calcGyro(imu.gz);
-            t1 = t2;
-            t2 = t.read();
-        }
-        t.stop();
+        _leftEncoder.reset();
+        _rightEncoder.reset();
+        leftWheel.speed(1.8);
+        rightWheel.speed(-1.8);
+        while((_leftEncoder.read() < 1) && (_rightEncoder.read() < 1));
         leftWheel.speed(0);
         rightWheel.speed(0);
-        obstacles[i] = 0; // TODO ping lidar
+        lidarBoard->sensor_centre->get_distance(&lidarDistance);
+        obstacles[i] = (int)(lidarDistance*MMTOIN);
     }
     led = 0;
 }
@@ -54,21 +56,41 @@
     led = 0b1000;
     while(!pb);
     for (int i = 0; i < trajectoryLength; i = i + 2) {
-        leftEncoder.reset();
-        rightEncoder.reset();
+        t.reset();
+        yaw = 0.0;
+        w1 = 0.0;
+        w2 = 0.0;
+        t1 = 0.0;
+        t2 = 0.0;
+        int angle = trajectory[i];
+        if ((trajectory[i] >= 0) && (trajectory[i] < 90)) {
+            angle = angle*ROTERRI;
+        } else if (trajectory[i] < 180) {
+            angle = angle*ROTERRII;
+        } else if (trajectory[i] < 360) {
+            angle = angle*ROTERRIII;
+        }
+        t.start();
         leftWheel.speed(0.2);
         rightWheel.speed(-0.2);
-        int angleCount = (int)(trajectory[i]*COUNTPERDEG);
-        while((leftEncoder.read() < angleCount) && (rightEncoder.read() < angleCount));
+        while(yaw < angle) {
+            yaw = yaw + (((w2+w1)/2.0)*(t2-t1));
+            while(!imu.gyroAvailable());
+            imu.readGyro();
+            w1 = w2;
+            w2 = imu.calcGyro(imu.gz);
+            t1 = t2;
+            t2 = t.read();
+        }
         leftWheel.speed(0);
         rightWheel.speed(0);
+        t.stop();
         leftEncoder.reset();
         rightEncoder.reset();
-        int distanceCount = (int)(trajectory[i + 1]*COUNTPERIN);
+        int distance = (int)(trajectory[i + 1]*COUNTPERIN);
         leftWheel.speed(0.2);
         rightWheel.speed(0.2);
-        pc.putc(distanceCount);
-        while((leftEncoder.read() < distanceCount) && (rightEncoder.read() < distanceCount));
+        while((leftEncoder.read() < distance) && (rightEncoder.read() < distance));
         leftWheel.speed(0);
         rightWheel.speed(0);
     }