Diff: Drive.cpp

Revision:

4:e3f388933954

Parent:

3:2ec7cf8bc3fc

Child:

5:b8b1a979b0d5

--- a/Drive.cpp	Sun Apr 08 19:17:01 2018 +0000
+++ b/Drive.cpp	Thu Apr 12 16:14:02 2018 +0000
@@ -7,18 +7,20 @@
 using namespace std;
 
 const float Drive::DRIVINGSPEED = 100.0f;//Fahrgeschwindigkeit
-const int Drive::DRIVINGCOUNTS = 1712;  //Entspricht Strecke von 20cm
+const int Drive::DRIVINGCOUNTS = 1712000;  //Entspricht Strecke von 20cm
 
-Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, float distanceRight, float distanceFront, float distanceLeftFront, float distanceLeftBack):
+Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3):
     kontrastSensor(kontrastSensor),
     counterLeft(counterLeft), 
     counterRight(counterRight),
-    controller(controller)
+    controller(controller),
+    irSensor0(irSensor0),
+    irSensor1(irSensor1),
+    irSensor2(irSensor2),
+    irSensor3(irSensor3)
+    
 {
-    this->distanceRight = distanceRight;
-    this->distanceFront = distanceFront; 
-    this->distanceLeftFront = distanceLeftFront;
-    this->distanceLeftBack = distanceLeftBack;
+
 }
 
 Drive::~Drive() {}
@@ -31,42 +33,94 @@
     int countsRight0 = countsRight;         //ReferenzCounts setzten
     int countsLeft = counterLeft.read();
     int countsLeft0 = countsLeft;
-    int totCLeft, totCRight, counter, sumCorrection;
-    int corLeft = 0;
-    int corRight = 0;
+    //int totCLeft, totCRight, 
+    int sumCorrection =0;
+    //int sumCor = 0;
+    float corLeft = 0;
+    float corRight = 0;
+    //int counter = 0;
     
     
-    printf("Los gehts\n");
+    
+    
+    //printf("Los gehts\n");
         
-    while((countsRight <= countsRight0 + DRIVINGCOUNTS) && (countsLeft >= countsLeft0 - DRIVINGCOUNTS) || distanceFront < 1.5f){ //evt auch noch wand vorne ermitteln und so!!!!!!!!!!!!!
+    while((countsRight <= countsRight0 + DRIVINGCOUNTS) && (countsLeft >= countsLeft0 - DRIVINGCOUNTS) ){ //evt auch noch wand vorne ermitteln und so!!!!!!!!!!!!! || distanceFront > 35.0f
     
         //kontrastSensor.check();
-        controller.setDesiredSpeedRight(DRIVINGSPEED + corRight);
+        controller.setDesiredSpeedRight(DRIVINGSPEED - corRight); //Korrektur verringert Geschwindigkeit
         controller.setDesiredSpeedLeft(-DRIVINGSPEED + corLeft);
         countsRight = counterRight.read();
         countsLeft = counterLeft.read();
         //printf("CountsRight%d\n\r", countsRight);
-        //printf("CountsLeft%d\n\r", countsLeft);
+        //printf("           CountsLeft%d\n\r", countsLeft);
+        
+        //totCLeft =+countsLeft;
+        //totCRight =+countsRight;    
         
-        totCLeft =+countsLeft;
-        totCRight =+countsRight;    
-        counter++;
+        if((irSensor3.read() < 100.0f) && (irSensor2.read() < 100.0f)){   // irSensor0 durch irSensor3 ersetzt
+            
+//            int i = 0;
+//            while (i < 10){
+//                
+//                sumCorrection += irSensor3.read()-irSensor2.read(); //irSensor0.read() => sensorRight irSensor2.read() => sensorLeftFornt    // irSensor0 durch irSensor3 ersetzt
+//                //sumCor = irSensor2.read()-irSensor3.read();
+//                //double AvSensDist;
+//                i++;
+//            }
+//            sumCorrection = sumCorrection/(double)i;
+            sumCorrection = irSensor2.read()-irSensor3.read();
+            printf("                                        SumCorrection:  %d\n\r", sumCorrection);
+            printf("                    DistanzLinksVorne = %.0f mm\n\r", irSensor2.read());
+            printf("DistanzLinksHinten = %.0f mm\n\r", irSensor3.read());  
+        }else{
+            corRight=0;
+            corLeft=0;
+        }
         
-        if(counter < 100){  //Wait Time until corretion value added
-            sumCorrection = totCLeft-totCRight;
-            if(sumCorrection <0){
-                corLeft =+1; //Driving Speed Left Tire Addition
+        //if(counter > 10){  //Wait Time until corretion value added
+            
+            
+            
+            if(sumCorrection < -1){    // (steht naeher an rechter Wand) Arsch zu weit links
+                //corLeft = 1 * -sumCorrection; //Driving Speed Left Tire Addition
+                corLeft = 20.0f;            // Correction neu 20 statt 1
                 corRight=0;
-            }else if(sumCorrection >0){
+            }else if(sumCorrection > 1){    // (steht naeher an linker Wand) Arsch zu weit rechts
                 corLeft=0;
-                corRight=+1; //Driving Speed Right Tire Addition   
+                corRight = 20.0f;           // Correction neu 20 statt 1
+                //corRight = 1 * sumCorrection; //Driving Speed Right Tire Addition   
+            }else{
+              
+              
+                    corLeft=0;
+                    corRight=0;
+                
+                     
+              //  if(sumCor < -2){
+              //      corLeft=0;
+              //      corRight = 1 * sumCor; //Driving Speed Right Tire Addition
+                    
+              //  }else if(sumCor > 2){
+              //      corLeft = 1 * -sumCor; //Driving Speed Left Tire Addition
+              //      corRight=0;
+              //  }else{
+                    
+              //      corLeft=0;
+              //      corRight=0;
+              //  }
             }
-            counter = 0;
-            totCLeft = 0;
-            totCRight = 0;
-        }
+                
+            //counter = 0;
+            sumCorrection =0;
+           // totCLeft = 0;
+            //totCRight = 0;
+        //}
+        //counter++;
+        
+        
     }
-    controller.setDesiredSpeedRight(0.0f);
-    controller.setDesiredSpeedLeft(0.0f);
+    controller.setDesiredSpeedRight(0.0f); 
+    controller.setDesiredSpeedLeft(0.0f); 
 
 }