Diff: Motion.cpp

Revision:

8:862bf9225953

Parent:

5:47262622a9bf

--- a/Motion.cpp	Mon Apr 16 12:49:50 2018 +0000
+++ b/Motion.cpp	Mon Apr 16 15:23:42 2018 +0000
@@ -30,17 +30,33 @@
  */
 void Motion::move() {
     
-        //distanceL = irSensorL.read();
-        //distanceC = irSensorC.read();
-        //distanceR = irSensorR.read();
+        //distanceL = irSensorL.readL();
+        //distanceC = irSensorC.readC();
+        //distanceR = irSensorR.readR();
+        float korr = 1;
+        const float kp = 0.28f;
+        float speedRight = -50.0f;
+        float speedLeft = 50.0f;
         
         while ((countsL - countsLOld)  < 1630 || (countsR - countsROld) > -1630) {
             
             countsL = counterLeft.read();
             countsR = counterRight.read();
+            distanceL = irSensorL.readL();
+            distanceR = irSensorR.readR();
+            
+            korr = (distanceL-distanceR)*kp;
+            
+            if (korr <= 1){
+                speedRight = -50.0f * korr;
+                speedLeft = 50.0f
+                }
+                else
+                speedLeft = 50.0f * -korr;
+                speedRight = -50.0f;
         
-            controller.setDesiredSpeedLeft(50.0f);
-            controller.setDesiredSpeedRight(-50.0f);
+            controller.setDesiredSpeedLeft(speedLeft);
+            controller.setDesiredSpeedRight(speedRight);
             enableMotorDriver = 1;
         }    
         //printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR);