Diff: main.cpp

Revision:

8:c59f12a4960f

Parent:

7:aa95fa560952

Child:

9:431b67beb44d

--- a/main.cpp	Mon Apr 27 20:43:58 2020 +0000
+++ b/main.cpp	Mon Apr 27 20:53:59 2020 +0000
@@ -114,6 +114,7 @@
             }
             leftEncoderCount = 0; //reset encoder counts
             rightEncoderCount = 0;
+            Thread:yield();
         }
         else if(encoderCountDifference < (-1 * countDifferenceTolerance)) //if left encoder counted less, right motor is faster
         {
@@ -131,8 +132,14 @@
             }
             leftEncoderCount = 0; //reset encoder counts
             rightEncoderCount = 0;
+            Thread::yield();
         }
-        else Thread::yield(); //if counts are same, nothing left to do
+        else //if counts within tolerances, keep speeds the same
+        {
+            rightMotor.speed(rightMotorSpeed);
+            leftMotor.speed(leftMotorSpeed);
+            Thread::yield(); //if counts are same, nothing left to do
+        } 
     }
     else //The case where joysticks arent in the same direction
     {
@@ -150,6 +157,7 @@
             leftEncoderCount = 0;
             rightEncoderCount = 0;
         }
+        Thread::yield();
     }
     }
 }
@@ -213,6 +221,5 @@
                 }
             }
         }
-        Thread::yield();
     }
 }