Revision 4:e3f388933954, committed 2018-04-12

Comitter:

ruesipat

Date:

Thu Apr 12 16:14:02 2018 +0000

Parent:

3:2ec7cf8bc3fc

Commit message:

Version12.04.18

Changed in this revision

--- 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); 
 
 }

--- a/Drive.h	Sun Apr 08 19:17:01 2018 +0000
+++ b/Drive.h	Thu Apr 12 16:14:02 2018 +0000
@@ -15,7 +15,7 @@
 
 public:
     
-    Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, float distanceRight, float distanceFront, float distanceLeftFront, float distanceLeftBack);
+    Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3);
     virtual ~Drive();
     void driving();
 private:
@@ -32,10 +32,10 @@
     
     Controller& controller;
     
-    float distanceRight;
-    float distanceFront;
-    float distanceLeftFront;
-    float distanceLeftBack;
+    IRSensor& irSensor0;
+    IRSensor& irSensor1;
+    IRSensor& irSensor2;
+    IRSensor& irSensor3;
     
 
 

--- a/Turn.cpp	Sun Apr 08 19:17:01 2018 +0000
+++ b/Turn.cpp	Thu Apr 12 16:14:02 2018 +0000
@@ -4,7 +4,7 @@
 using namespace std;
 
 const float Turn::TURNINGSPEED = 50.0f;//Drehgeschwindgkeit
-const int Turn::TURNINGCOUNTS = 941;  //Entspricht Drehung um 90Grad
+const int Turn::TURNINGCOUNTS = 946;  //Entspricht Drehung um 90Grad //941;
 
 Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft):
     counterLeft(counterLeft), 
@@ -29,26 +29,26 @@
     
     if (wallLeft == 0){ //Nach Links Drehen
     
-    printf("Links ist frei\n");
+    //printf("Links ist frei\n");
     
     while((countsRight <= countsRight0 + TURNINGCOUNTS) && (countsLeft <= countsLeft0 + TURNINGCOUNTS)){
         controller.setDesiredSpeedRight(TURNINGSPEED);
         controller.setDesiredSpeedLeft(TURNINGSPEED);
         countsRight = counterRight.read();
         countsLeft = counterLeft.read();
-        printf("%d\n\r", countsRight);
-        printf("%d\n\r", countsLeft);
+        //printf("%d\n\r", countsRight);
+        //printf("%d\n\r", countsLeft);
     }
     controller.setDesiredSpeedRight(0.0f);
     controller.setDesiredSpeedLeft(0.0f);
     
     }else if (wallFront == 0){ //Nicht Drehen-> weiter Geradeaus
     
-        printf("Vorne ist frei\n");
+        //printf("Vorne ist frei\n");
         
         }else if (wallRight == 0) { //Nach Rechts Drehen
             
-            printf("Rechts ist frei\n");
+            //printf("Rechts ist frei\n");
             
             while((countsRight >= countsRight0 - TURNINGCOUNTS) && (countsLeft >= countsLeft0 - TURNINGCOUNTS)){
                 controller.setDesiredSpeedRight(-TURNINGSPEED);
@@ -64,7 +64,7 @@
             
             }else{ //Alle Wege versperrt-> Wenden
             
-                printf("Alles versperrt...zurueck\n");
+                //printf("Alles versperrt...zurueck\n");
             
                 while((countsRight <= countsRight0 + 2*TURNINGCOUNTS) && (countsLeft <= countsLeft0 + 2*TURNINGCOUNTS)){
                     controller.setDesiredSpeedRight(TURNINGSPEED);

--- a/main.cpp	Sun Apr 08 19:17:01 2018 +0000
+++ b/main.cpp	Thu Apr 12 16:14:02 2018 +0000
@@ -37,7 +37,7 @@
 IRSensor irSensor0(distance0);    //rechts
 IRSensor irSensor1(distance1);    //vorne
 IRSensor irSensor2(distance2);    //links-vorne
-IRSensor irSensor3(distance3);    //links-hinten
+IRSensor irSensor3(distance3);    //links-hinten 
 
 
 //AnalogIn distance(PB_1); //Kreieren der Ein - und Ausgangsobjekte
@@ -74,7 +74,10 @@
 int main()
 {
 
-
+    int wallRight = 0;
+    int wallFront = 0;
+    int wallLeft = 0; 
+    int blackLine = 0;
     
 
     enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
@@ -101,16 +104,13 @@
             float distanceLeftFront = irSensor2.read();
             float distanceLeftBack = irSensor3.read();
              
-            int wallRight;
-            int wallFront;
-            int wallLeft; 
-            int blackLine;
+
             
             CheckWalls checkWalls(distanceRight, distanceFront, distanceLeftFront, wallRight, wallFront, wallLeft); //Ermittlung wo freie Wege(0) bzw welche Waende vorhanden (1)
             //Decide??
             Turn turn(counterLeft, counterRight, controller, wallRight, wallFront, wallLeft);   //Nach Ausrichtung bewegt sich der Roboter um ein Feld weiter und die Ausrichutung beginnt von vorne
             KontrastSensor kontrastSensor(kontrast, blackLine); 
-            Drive drive(kontrastSensor, counterLeft, counterRight, controller, distanceRight, distanceFront, distanceLeftFront, distanceLeftBack); //20cm nach vorne...
+            Drive drive(kontrastSensor, counterLeft, counterRight, controller, irSensor0, irSensor1, irSensor2, irSensor3); //20cm nach vorne...
                                                            
             
             switch(state) { //Ausrichten
@@ -127,7 +127,7 @@
                     //wallFront = 1;
                     //wallLeft = 1;
                     
-                    turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
+                    //turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
                     
 
                     drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden