Diff: Drive.cpp

Revision:

3:2ec7cf8bc3fc

Parent:

2:592f01278db4

Child:

4:e3f388933954

--- a/Drive.cpp	Wed Apr 04 15:24:28 2018 +0000
+++ b/Drive.cpp	Sun Apr 08 19:17:01 2018 +0000
@@ -3,6 +3,7 @@
 
 
 
+
 using namespace std;
 
 const float Drive::DRIVINGSPEED = 100.0f;//Fahrgeschwindigkeit
@@ -30,23 +31,40 @@
     int countsRight0 = countsRight;         //ReferenzCounts setzten
     int countsLeft = counterLeft.read();
     int countsLeft0 = countsLeft;
+    int totCLeft, totCRight, counter, sumCorrection;
+    int corLeft = 0;
+    int corRight = 0;
+    
     
     printf("Los gehts\n");
         
-    while((countsRight <= countsRight0 + DRIVINGCOUNTS) && (countsLeft >= countsLeft0 - DRIVINGCOUNTS)){ //evt auch noch wand vorne ermitteln und so!!!!!!!!!!!!!
+    while((countsRight <= countsRight0 + DRIVINGCOUNTS) && (countsLeft >= countsLeft0 - DRIVINGCOUNTS) || distanceFront < 1.5f){ //evt auch noch wand vorne ermitteln und so!!!!!!!!!!!!!
     
         //kontrastSensor.check();
-        controller.setDesiredSpeedRight(DRIVINGSPEED);
-        controller.setDesiredSpeedLeft(-DRIVINGSPEED);
+        controller.setDesiredSpeedRight(DRIVINGSPEED + corRight);
+        controller.setDesiredSpeedLeft(-DRIVINGSPEED + corLeft);
         countsRight = counterRight.read();
         countsLeft = counterLeft.read();
         //printf("CountsRight%d\n\r", countsRight);
         //printf("CountsLeft%d\n\r", countsLeft);
         
+        totCLeft =+countsLeft;
+        totCRight =+countsRight;    
+        counter++;
         
-
-        //(distanceLeftFront - distanceRight)  /rotationsgeschwindigkeit und allenfalls korrektur!!!!!!!!!!!!!!
-        
+        if(counter < 100){  //Wait Time until corretion value added
+            sumCorrection = totCLeft-totCRight;
+            if(sumCorrection <0){
+                corLeft =+1; //Driving Speed Left Tire Addition
+                corRight=0;
+            }else if(sumCorrection >0){
+                corLeft=0;
+                corRight=+1; //Driving Speed Right Tire Addition   
+            }
+            counter = 0;
+            totCLeft = 0;
+            totCRight = 0;
+        }
     }
     controller.setDesiredSpeedRight(0.0f);
     controller.setDesiredSpeedLeft(0.0f);