Diff: RobotController.cpp

Revision:

16:5c38e24db603

Parent:

14:0e6b26c1a7c5

Child:

17:e1a5d25218de

--- a/RobotController.cpp	Thu Apr 25 03:22:06 2019 +0000
+++ b/RobotController.cpp	Thu Apr 25 05:00:18 2019 +0000
@@ -30,8 +30,8 @@
 RobotController::~RobotController() {
     delete &leftWheel;
     delete &rightWheel;
-    delete &leftEncoder;
-    delete &rightEncoder;
+    delete &_leftEncoder;
+    delete &_rightEncoder;
     delete &imu;
     delete &t;
 }
@@ -41,8 +41,8 @@
     for (int i = 0; i < 360; i++) {
         _leftEncoder.reset();
         _rightEncoder.reset();
-        leftWheel.speed(1.8);
-        rightWheel.speed(-1.8);
+        leftWheel.speed(0.4);
+        rightWheel.speed(-0.4);
         while((_leftEncoder.read() < 1) && (_rightEncoder.read() < 1));
         leftWheel.speed(0);
         rightWheel.speed(0);
@@ -62,10 +62,10 @@
         w2 = 0.0;
         t1 = 0.0;
         t2 = 0.0;
-        int angle = trajectory[i];
-        if ((trajectory[i] >= 0) && (trajectory[i] < 90)) {
+        int angle = trajectory[i] % 360;
+        if ((trajectory[i] >= 0) && (trajectory[i] <= 90)) {
             angle = angle*ROTERRI;
-        } else if (trajectory[i] < 180) {
+        } else if (trajectory[i] <= 180) {
             angle = angle*ROTERRII;
         } else if (trajectory[i] < 360) {
             angle = angle*ROTERRIII;
@@ -85,12 +85,12 @@
         leftWheel.speed(0);
         rightWheel.speed(0);
         t.stop();
-        leftEncoder.reset();
-        rightEncoder.reset();
+        _leftEncoder.reset();
+        _rightEncoder.reset();
         int distance = (int)(trajectory[i + 1]*COUNTPERIN);
         leftWheel.speed(0.2);
         rightWheel.speed(0.2);
-        while((leftEncoder.read() < distance) && (rightEncoder.read() < distance));
+        while((_leftEncoder.read() < distance) && (_rightEncoder.read() < distance));
         leftWheel.speed(0);
         rightWheel.speed(0);
     }