Diff: RobotController.cpp

Revision:

19:41d0cc14edc5

Parent:

18:7ed985584ec7

Child:

20:d9a373fecd42

--- a/RobotController.cpp	Thu Apr 25 16:54:57 2019 +0000
+++ b/RobotController.cpp	Thu Apr 25 18:53:13 2019 +0000
@@ -27,16 +27,18 @@
 
 void RobotController::detectObstacles() {
     led = 0b0001;
+    while(!pb);
+    Thread::wait(500);
     for (int i = 0; i < 360; i++) {
         _leftEncoder.reset();
         _rightEncoder.reset();
-        leftWheel.speed(0.4);
-        rightWheel.speed(-0.4);
+        leftWheel.speed(0.2);
+        rightWheel.speed(-0.2);
+        Thread::wait(10);
         while((_leftEncoder.read() < 1) && (_rightEncoder.read() < 1));
         leftWheel.speed(0);
         rightWheel.speed(0);
         obstacles[i] = (int)(lidarDistance / 10);
-        pc.printf("Index %d, %d\n", i, lidarDistance);
     }
     led = 0;
 }
@@ -44,7 +46,8 @@
 void RobotController::followTrajectory() {
     led = 0b1000;
     while(!pb);
-    for (int i = 0; i < trajectoryLength; i = i + 2) {
+    Thread::wait(500);
+    for (int i = 0; i < trajectoryLength * 2; i = i + 2) {
         t.reset();
         yaw = 0.0;
         w1 = 0.0;
@@ -78,7 +81,7 @@
         useImu = false;
         _leftEncoder.reset();
         _rightEncoder.reset();
-        int distance = (int)(trajectory[i + 1]*COUNTPERCM);
+        int distance = (int)(trajectory[i + 1] * COUNTPERCM / 3);
         leftWheel.speed(0.2);
         rightWheel.speed(0.2);
         while((_leftEncoder.read() < distance) && (_rightEncoder.read() < distance));