Diff: Drive.cpp

Revision:

5:b8b1a979b0d5

Parent:

4:e3f388933954

Child:

6:a09d2ee3b82e

--- a/Drive.cpp	Thu Apr 12 16:14:02 2018 +0000
+++ b/Drive.cpp	Wed Apr 25 12:07:03 2018 +0000
@@ -1,4 +1,5 @@
 #include <cmath>
+#include "mbed.h"
 #include "Drive.h"
 
 
@@ -6,19 +7,20 @@
 
 using namespace std;
 
-const float Drive::DRIVINGSPEED = 100.0f;//Fahrgeschwindigkeit
-const int Drive::DRIVINGCOUNTS = 1712000;  //Entspricht Strecke von 20cm
+const float Drive::FRONTDISTANCE = 62.0f; //Abstand Sensor zur VorderWand //DONT TOUCH
+const float Drive::DRIVINGSPEED = 100.0f;//Fahrgeschwindigkeit  Drehzahl in [rpm]
+const int Drive::DRIVINGCOUNTS = 1773;  //Entspricht Strecke von 20cm  //DONT TOUCH
 
 Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3):
     kontrastSensor(kontrastSensor),
-    counterLeft(counterLeft), 
+    counterLeft(counterLeft),
     counterRight(counterRight),
     controller(controller),
     irSensor0(irSensor0),
     irSensor1(irSensor1),
     irSensor2(irSensor2),
     irSensor3(irSensor3)
-    
+
 {
 
 }
@@ -29,98 +31,111 @@
 void Drive::driving()
 {
 
-    int countsRight = counterRight.read();  //EncoderCounts auslesen  
-    int countsRight0 = countsRight;         //ReferenzCounts setzten
-    int countsLeft = counterLeft.read();
-    int countsLeft0 = countsLeft;
-    //int totCLeft, totCRight, 
-    int sumCorrection =0;
-    //int sumCor = 0;
-    float corLeft = 0;
-    float corRight = 0;
-    //int counter = 0;
+    controller.reset();
     
-    
-    
-    
+    int countsRight = counterRight.read();  //EncoderCounts auslesen
+    int countsLeft = counterLeft.read();        //0 - 32767
+
+    //printf("CountsRight%d\n\r", countsRight);
+    //printf("           CountsLeft%d\n\r", countsLeft);
+
+
+    int countsRight0 = countsRight;         //ReferenzCounts setzten
+    int countsLeft0 = countsLeft;
+
+    float parallelDif = 0;
+    float rightLeftDif = 0;
+
+    float correction = 0;
+    float slowdown = 0;
+
+    int drive;
+
+
+    //Abfangen wenn Wand vorne dass sicher nicht vorwärts gefahren wird
+
+    if(irSensor1.read() < FRONTDISTANCE) {
+        drive = 0;
+    } else {
+        drive = 1;
+    }
+
+
     //printf("Los gehts\n");
-        
-    while((countsRight <= countsRight0 + DRIVINGCOUNTS) && (countsLeft >= countsLeft0 - DRIVINGCOUNTS) ){ //evt auch noch wand vorne ermitteln und so!!!!!!!!!!!!! || distanceFront > 35.0f
-    
+
+    while(((countsRight <= countsRight0 + DRIVINGCOUNTS) || (countsLeft >= countsLeft0 - DRIVINGCOUNTS)) && (drive == 1) ) {
+
         //kontrastSensor.check();
-        controller.setDesiredSpeedRight(DRIVINGSPEED - corRight); //Korrektur verringert Geschwindigkeit
-        controller.setDesiredSpeedLeft(-DRIVINGSPEED + corLeft);
         countsRight = counterRight.read();
         countsLeft = counterLeft.read();
+        controller.setDesiredSpeedRight(DRIVINGSPEED - correction - slowdown); //Korrektur passt Geschwindigkeit an beiden Raedern an
+        controller.setDesiredSpeedLeft(-DRIVINGSPEED - correction + slowdown);
+
         //printf("CountsRight%d\n\r", countsRight);
         //printf("           CountsLeft%d\n\r", countsLeft);
-        
-        //totCLeft =+countsLeft;
-        //totCRight =+countsRight;    
+
+
+        //printf("correction:  %.0f\n\r", correction);
+
+
+        //Bereit fuer neuen Durchgang
+        correction = 0;
+        slowdown = 0;
+
+
+        //Ermittlung der Differenz Hinten-Vorne
+        if((irSensor3.read() < 100.0f) && (irSensor2.read() < 100.0f)) { //irSensor3 => sensorLeftBack , irSensor2 => sensorLeftFront
         
-        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 > 10){  //Wait Time until corretion value added
-            
-            
+            parallelDif = irSensor3.read()-irSensor2.read();    //differenz hinten vorne bestimmen
+            //printf("        DistanzLinksVorne = %.0f mm\n\r", irSensor2.read());
+            //printf("        DistanzLinksHinten = %.0f mm\n\r", irSensor3.read());
+            //printf("            parallelDif:  %.0f \n\r", parallelDif);
             
-            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 > 1){    // (steht naeher an linker Wand) Arsch zu weit rechts
-                corLeft=0;
-                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;
-              //  }
+        } else { //ist wand eine wand nicht vorhanden => keine korrektur
+        
+            parallelDif = 0;
+        }
+
+
+        //Ermittlung der Differenz Rechts-Links
+        if((irSensor0.read() < 100.0f) && (irSensor2.read() < 100.0f)) {  //irSensor0 => sensorRight irSensor2 => sensorLeftFornt
+        
+            rightLeftDif = irSensor0.read()-irSensor2.read(); //differenz links rechts bestimmen
+            //printf("                DistanzRechts = %.0f mm\n\r", irSensor0.read());
+            //printf("                DistanzLinksHinten = %.0f mm\n\r", irSensor2.read());
+            //printf(" rightLeftDif:  %.0f \n\r", rightLeftDif);
+
+        } else { //ist wand eine wand nicht vorhanden => keine korrektur
+
+            rightLeftDif = 0;
+
+        }
+
+
+
+        //Berechung Korrektur
+        correction = ((0.1f * rightLeftDif) + (0.5f * parallelDif)); //DONT TOUCH
+
+
+
+
+        //Kontrolle ob vorne Wand...Verlangsamen und Anhalten
+
+        if(irSensor1.read() < 150.0f) { //slow down
+
+            slowdown = FRONTDISTANCE/irSensor1.read() * DRIVINGSPEED;           //vorderer max abstand
+
+            if ( slowdown > DRIVINGSPEED) {
+
+                drive = 0;
+
             }
-                
-            //counter = 0;
-            sumCorrection =0;
-           // totCLeft = 0;
-            //totCRight = 0;
-        //}
-        //counter++;
-        
-        
-    }
-    controller.setDesiredSpeedRight(0.0f); 
-    controller.setDesiredSpeedLeft(0.0f); 
+        }
+
+
+    }//Ende Whileschleife Drive...
+
+    controller.setDesiredSpeedRight(0.0f);
+    controller.setDesiredSpeedLeft(0.0f);
 
 }