Diff: Drive.cpp

Revision:

7:5ff551b098f8

Parent:

6:a09d2ee3b82e

--- a/Drive.cpp	Wed Apr 25 12:49:48 2018 +0000
+++ b/Drive.cpp	Wed May 02 11:53:42 2018 +0000
@@ -7,9 +7,9 @@
 
 using namespace std;
 
-const float Drive::FRONTDISTANCE = 62.0f; //Abstand Sensor zur VorderWand //DONT TOUCH
-const float Drive::DRIVINGSPEED = 50.0f;//Fahrgeschwindigkeit  Drehzahl in [rpm]
-const int Drive::DRIVINGCOUNTS = 1773;  //Entspricht Strecke von 20cm  //DONT TOUCH
+const float Drive::FRONTDISTANCE = 80.0f; //Abstand Sensor zur VorderWand //DONT TOUCH  //62.0f //55.0
+const float Drive::DRIVINGSPEED = 100.0f;//Fahrgeschwindigkeit  Drehzahl in [rpm]
+const int Drive::DRIVINGCOUNTS = 1480;  //Entspricht Strecke von 20cm  //DONT TOUCH /1773 //1800
 
 Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3):
     kontrastSensor(kontrastSensor),
@@ -46,8 +46,10 @@
     float parallelDif = 0;
     float rightLeftDif = 0;
 
-    float correction = 0;
+    float speedCorrection = 0;
+    
     float slowdown = 0;
+    int countCorrection = 0;
 
     int drive;
 
@@ -63,13 +65,13 @@
 
     //printf("Los gehts\n");
 
-    while(((countsRight <= countsRight0 + DRIVINGCOUNTS) || (countsLeft >= countsLeft0 - DRIVINGCOUNTS)) && (drive == 1) ) {
+    while(((countsRight <= countsRight0 + DRIVINGCOUNTS + countCorrection) || (countsLeft >= countsLeft0 - DRIVINGCOUNTS - countCorrection)) && (drive == 1) ) {
 
         kontrastSensor.check();
         countsRight = counterRight.read();
         countsLeft = counterLeft.read();
-        controller.setDesiredSpeedRight(DRIVINGSPEED - correction - slowdown); //Korrektur passt Geschwindigkeit an beiden Raedern an
-        controller.setDesiredSpeedLeft(-DRIVINGSPEED - correction + slowdown);
+        controller.setDesiredSpeedRight(DRIVINGSPEED - speedCorrection - slowdown/3); //Korrektur passt Geschwindigkeit an beiden Raedern an
+        controller.setDesiredSpeedLeft(-DRIVINGSPEED - speedCorrection + slowdown/3);
 
         //printf("CountsRight%d\n\r", countsRight);
         //printf("           CountsLeft%d\n\r", countsLeft);
@@ -79,7 +81,7 @@
 
 
         //Bereit fuer neuen Durchgang
-        correction = 0;
+        speedCorrection = 0;
         slowdown = 0;
 
 
@@ -114,7 +116,7 @@
 
 
         //Berechung Korrektur
-        correction = ((0.1f * rightLeftDif) + (0.5f * parallelDif)); //DONT TOUCH
+        speedCorrection = ((0.2f * rightLeftDif) + (0.5f * parallelDif)); //DONT TOUCH
 
 
 
@@ -123,6 +125,7 @@
 
         if(irSensor1.read() < 150.0f) { //slow down
 
+            countCorrection = 500;                                              // ermoeglicht an eine gesichtete wand anzugleichen
             slowdown = FRONTDISTANCE/irSensor1.read() * DRIVINGSPEED;           //vorderer max abstand
 
             if ( slowdown > DRIVINGSPEED) {
@@ -135,7 +138,7 @@
 
     }//Ende Whileschleife Drive...
 
-    controller.setDesiredSpeedRight(0.5f);  //0.0f
-    controller.setDesiredSpeedLeft(0.5f);   //0.0f
+    //controller.setDesiredSpeedRight(0.5f);  //0.5f
+    //controller.setDesiredSpeedLeft(-0.5f);   //-0.5f
 
 }