Diff: Motion.cpp

Revision:

4:932eb2d29206

Parent:

3:076dd7ec7eb4

Child:

5:e2c0a4388d85

--- a/Motion.cpp	Fri Apr 20 13:37:21 2018 +0000
+++ b/Motion.cpp	Mon Apr 23 12:14:54 2018 +0000
@@ -6,7 +6,7 @@
 
 const float Motion::LEFT_MID_VAL = 44.73f;
 const float Motion::RIGHT_MID_VAL = 47.03f;
-const float Motion::KP = 3.0f;
+const float Motion::KP = 2.0f;
 const float Motion::KD = 0.00f;
 
 
@@ -38,7 +38,7 @@
         controller.setDesiredSpeedLeft(speedLeft);
         controller.setDesiredSpeedRight(speedRight);
         
-        while ((countsL - countsLOld)  < 1647 || (countsR - countsROld) > -1647) {
+        while ((countsL - countsLOld)  < 1565 || (countsR - countsROld) > -1565) {
             
             countsL = counterLeft.read();
             countsR = counterRight.read();
@@ -94,7 +94,7 @@
         controller.setDesiredSpeedLeft(speedLeft);
         controller.setDesiredSpeedRight(speedRight);
         
-        while ((countsL - countsLOld)  <  1647 || (countsR - countsROld) > -1647) {
+        while ((countsL - countsLOld)  <  1565 || (countsR - countsROld) > -1565) {
             
             countsL = counterLeft.read();
             countsR = counterRight.read();
@@ -106,8 +106,6 @@
             
             if (lineSensor.read() == 1.0f) {
                 line = 1;  
-            }else{
-                line = 0;
             }
             
             control();
@@ -260,20 +258,23 @@
 
 void Motion::control() {
         
-    //float wallLeft = 70.0f;
-    //float wallRight = 70.0f;
+    float wallLeft = 80.0f;
+    float wallRight = 80.0f;
     
     distanceL = irSensorL.readL();
     distanceR = irSensorR.readR();
     
-    if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) {
+    if (distanceL < wallLeft && distanceR < wallRight) {
+        /*if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) {
+            
+            errorP = LEFT_MID_VAL - distanceL;
+               
+        }else if (distanceL > LEFT_MID_VAL && distanceR < RIGHT_MID_VAL) {
+            
+            errorP = distanceR - RIGHT_MID_VAL;
+        }*/
         
-        errorP = LEFT_MID_VAL - distanceL;
-           
-    }else if (distanceL > LEFT_MID_VAL && distanceR < RIGHT_MID_VAL) {
-        
-        errorP = distanceR - RIGHT_MID_VAL;
-         
+        errorP = distanceL - distanceR + 2.30f;
     }else{
         
         errorP = 0;