Diff: RobotController.cpp

Revision:

11:531208aca075

Parent:

8:bfa4bf23522c

Child:

14:0e6b26c1a7c5

--- a/RobotController.cpp	Mon Apr 22 23:55:45 2019 +0000
+++ b/RobotController.cpp	Tue Apr 23 19:56:39 2019 +0000
@@ -2,10 +2,27 @@
 
 RobotController::RobotController(PinName leftWheelPwm, PinName leftWheelFwd,
     PinName leftWheelRev, PinName rightWheelPwm, PinName rightWheelFwd,
-    PinName rightWheelRev, PinName leftEncoder, PinName rightEncoder) :
+    PinName rightWheelRev, PinName leftEncoder, PinName rightEncoder,
+    PinName imuSda, PinName imuScl) :
     leftWheel(leftWheelPwm, leftWheelFwd, leftWheelRev),
     rightWheel(rightWheelPwm, rightWheelFwd, rightWheelRev),
-    leftEncoder(leftEncoder), rightEncoder(rightEncoder) {
+    leftEncoder(leftEncoder), rightEncoder(rightEncoder),
+    imu(imuSda, imuScl, 0xD6, 0x3C) {
+        imu.begin();
+        imu.calibrate();
+        w1 = 0.0;
+        w2 = 0.0;
+        t1 = 0.0;
+        t2 = 0.0;
+}
+
+RobotController::~RobotController() {
+    delete &leftWheel;
+    delete &rightWheel;
+    delete &leftEncoder;
+    delete &rightEncoder;
+    delete &imu;
+    delete &t;
 }
 
 void RobotController::detectObstacles() {
@@ -13,9 +30,19 @@
     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((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();
         leftWheel.speed(0);
         rightWheel.speed(0);
         obstacles[i] = 0; // TODO ping lidar