PES2_R2D2.0 / Mbed 2 deprecated MicroMouse_MASTER_TWO

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of Micromouse_alpha_copy_copy by PES2_R2D2.0

Files at this revision

API Documentation at this revision

Revision 7:5ef09519a6e9, committed 2018-05-07

Comitter:
ruesipat
Date:
Mon May 07 18:52:04 2018 +0000
Parent:
6:a09d2ee3b82e
Commit message:
VERSION 7.5.18

Changed in this revision

Drive.cpp Show annotated file Show diff for this revision Revisions of this file
Drive.h Show annotated file Show diff for this revision Revisions of this file
KontrastSensor.cpp Show annotated file Show diff for this revision Revisions of this file
Turn.cpp Show annotated file Show diff for this revision Revisions of this file
Turn.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/Drive.cpp	Wed Apr 25 12:49:48 2018 +0000
+++ b/Drive.cpp	Mon May 07 18:52:04 2018 +0000
@@ -7,11 +7,11 @@
 
 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 = 70.0f; //Abstand Sensor zur VorderWand //DONT TOUCH  //62.0f //55.0 110.0
+const float Drive::DRIVINGSPEED = 130.0f;//Fahrgeschwindigkeit  Drehzahl in [rpm]       //130.0f
+const int Drive::DRIVINGCOUNTS = 1390;  //Entspricht Strecke von 20cm  //DONT TOUCH /1773 //1800 //1350 /1370
 
-Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3):
+Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3, int& dontStop):
     kontrastSensor(kontrastSensor),
     counterLeft(counterLeft),
     counterRight(counterRight),
@@ -19,7 +19,8 @@
     irSensor0(irSensor0),
     irSensor1(irSensor1),
     irSensor2(irSensor2),
-    irSensor3(irSensor3)
+    irSensor3(irSensor3),
+    dontStop(dontStop)
 
 {
 
@@ -32,7 +33,7 @@
 {
 
     controller.reset();
-    
+
     int countsRight = counterRight.read();  //EncoderCounts auslesen
     int countsLeft = counterLeft.read();        //0 - 32767
 
@@ -43,11 +44,23 @@
     int countsRight0 = countsRight;         //ReferenzCounts setzten
     int countsLeft0 = countsLeft;
 
-    float parallelDif = 0;
-    float rightLeftDif = 0;
+    float parallelDif = 0.0f;
+    float rightLeftDif = 0.0f;
+    float leftDif = 0.0f;
+    float rightDif = 0.0f;
 
-    float correction = 0;
-    float slowdown = 0;
+    float speedCorrection = 0.0f;
+    int countCorrection = 0;
+    
+    
+    float softStart = DRIVINGSPEED;
+    float slowdown = 0.0f;
+
+    
+    if(dontStop == 2){ //geradeaus
+        softStart =0.0f;
+    }
+    
 
     int drive;
 
@@ -63,13 +76,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(-softStart + DRIVINGSPEED - speedCorrection - slowdown); //Korrektur passt Geschwindigkeit an beiden Raedern an //slowdown
+        controller.setDesiredSpeedLeft(softStart - DRIVINGSPEED - speedCorrection + slowdown);  //slowdown
 
         //printf("CountsRight%d\n\r", countsRight);
         //printf("           CountsLeft%d\n\r", countsLeft);
@@ -78,28 +91,31 @@
         //printf("correction:  %.0f\n\r", correction);
 
 
+
         //Bereit fuer neuen Durchgang
-        correction = 0;
-        slowdown = 0;
+        //speedCorrection = 0.0f;
+        //leftDif = 0.0f;
+        //rightDif = 0.0f;
+        //slowdown = 0.0f;
 
 
-        //Ermittlung der Differenz Hinten-Vorne
-        if((irSensor3.read() < 100.0f) && (irSensor2.read() < 100.0f)) { //irSensor3 => sensorLeftBack , irSensor2 => sensorLeftFront
-        
+        //Ermittlung der Differenz Hinten-Vorne (PARALLEL)
+        if((irSensor3.read() < 120.0f) && (irSensor2.read() < 120.0f)) { //irSensor3 => sensorLeftBack , irSensor2 => sensorLeftFront
+
             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);
-            
+
         } 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
-        
+        //Ermittlung der Differenz Rechts-Links (LINKS UND RECHTS WAND VORHANDEN)
+        if((irSensor0.read() < 120.0f) && (irSensor2.read() < 120.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());
@@ -109,33 +125,96 @@
 
             rightLeftDif = 0;
 
+            //Ermittlung der Differenz links auf ReferenzWert
+            if(irSensor2.read() < 120.0f) {  //irSensor2 => sensorLeftFornt
+
+                leftDif = 60.0f - irSensor2.read(); //differenz links rechts bestimmen
+                //printf("                DistanzRechts = %.0f mm\n\r", irSensor0.read());
+                //printf("                DistanzLinksHinten = %.0f mm\n\r", irSensor2.read());
+                //printf(" leftDif:  %.0f \n\r", leftDif);
+
+            } else { //ist wand eine wand nicht vorhanden => keine korrektur
+
+                leftDif = 0;
+
+            }
+
+
+            //Ermittlung der Differenz rechts auf Referenzwert
+            if(irSensor0.read() < 120.0f) {  //irSensor0 => sensorRight
+
+                rightDif = irSensor0.read() - 60.0f; //differenz links rechts bestimmen
+                //printf("                DistanzRechts = %.0f mm\n\r", irSensor0.read());
+                //printf("                DistanzLinksHinten = %.0f mm\n\r", irSensor2.read());
+                //printf(" rightDif:  %.0f \n\r", rightDif);
+
+            } else { //ist wand eine wand nicht vorhanden => keine korrektur
+
+                rightDif = 0;
+
+            }
+
+
+
+
         }
 
 
 
         //Berechung Korrektur
-        correction = ((0.1f * rightLeftDif) + (0.5f * parallelDif)); //DONT TOUCH
+        speedCorrection = ((0.35f * rightLeftDif) + (0.7f * parallelDif) + (0.7f * leftDif) + (0.7f * rightDif)); //DONT TOUCH 0.2 0.5
+
+
+
+        //Anfahrrampe damit die Raeder nicht durchdrehen
+        softStart = softStart - 3.5f;
+        if (softStart <= 0.0f) {
+            softStart = 0.0f;
+        }
+        //printf("           softStart: %.0f\n\r", softStart);
+
+
+
 
 
 
 
-        //Kontrolle ob vorne Wand...Verlangsamen und Anhalten
-
+        //Kontrolle ob vorne Wand...wenn vorhanden nach vorderer Wand ausrichten und anhalten ansonst nur nach counts anhalten
         if(irSensor1.read() < 150.0f) { //slow down
 
-            slowdown = FRONTDISTANCE/irSensor1.read() * DRIVINGSPEED;           //vorderer max abstand
-
-            if ( slowdown > DRIVINGSPEED) {
+            countCorrection = 5000;
+            //slowdown = FRONTDISTANCE/irSensor1.read() * DRIVINGSPEED;           //vorderer max abstand
+            slowdown = slowdown + 5.0f;
+            if (slowdown >= DRIVINGSPEED - 20.0f) {
+                slowdown = DRIVINGSPEED - 20.0f;
+            }
+            if (irSensor1.read() < FRONTDISTANCE) {
 
                 drive = 0;
 
             }
+            
+        } else if(((DRIVINGCOUNTS - countsRight) < 300 || (countsLeft + DRIVINGCOUNTS) < 300) && (irSensor2.read() > 120.0f)) { //Anhaltrampe wenn nach counts gefahren
+            slowdown = slowdown + 5.0f;
+            if (slowdown >= DRIVINGSPEED - 20.0f) {
+                slowdown = DRIVINGSPEED - 20.0f;
+            }
+            
+        } else{
+            slowdown = 0.0f;
         }
+        
+        
+        //if(dontStop == 2){ //geradeaus
+        //    slowdown = 0.0f;
+        //}
 
 
+
+        wait(0.01f);
     }//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
 
 }

--- a/Drive.h	Wed Apr 25 12:49:48 2018 +0000
+++ b/Drive.h	Mon May 07 18:52:04 2018 +0000
@@ -15,7 +15,7 @@
 
 public:
     
-    Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3);
+    Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3, int& dontStop);
     virtual ~Drive();
     void driving();
 private:
@@ -38,6 +38,8 @@
     IRSensor& irSensor2;
     IRSensor& irSensor3;
     
+    int& dontStop;
+    
 
 
 };

--- a/KontrastSensor.cpp	Wed Apr 25 12:49:48 2018 +0000
+++ b/KontrastSensor.cpp	Mon May 07 18:52:04 2018 +0000
@@ -18,10 +18,10 @@
 {
 
     
-    float k = kontrast*3300; //sodass in Range bis 3300mV
-    printf("%.0f\n\r", k);
+    float k = kontrast*3300.0f; //sodass in Range bis 3300mV
+    //printf("%.0f\n\r", k);
     
-    if(k >1300){
+    if(k >2500.0f){
         blackLine = 1;
     }
     //else{//Auschalten im Betrieb (nur zur Kontrolle)

--- a/Turn.cpp	Wed Apr 25 12:49:48 2018 +0000
+++ b/Turn.cpp	Mon May 07 18:52:04 2018 +0000
@@ -3,16 +3,17 @@
 
 using namespace std;
 
-const float Turn::TURNINGSPEED = 50.0f;//Drehgeschwindgkeit Drehzahl in [rpm]
-const int Turn::TURNINGCOUNTS = 868;  //Entspricht Drehung um 90Grad //DONT TOUCH
+const float Turn::TURNINGSPEED = 70.0f;//Drehgeschwindgkeit Drehzahl in [rpm]
+const int Turn::TURNINGCOUNTS = 950;  //Entspricht Drehung um 90Grad //DONT TOUCH //940//1070
 
-Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft):
+Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft, int& dontStop):
     counterLeft(counterLeft), 
     counterRight(counterRight),
     controller(controller),
     wallRight(wallRight),
     wallFront(wallFront),
-    wallLeft(wallLeft)
+    wallLeft(wallLeft),
+    dontStop(dontStop)
 {}
 
 Turn::~Turn() {}
@@ -25,6 +26,13 @@
     int countsLeft = counterLeft.read();
     int countsLeft0 = countsLeft;
     
+    //vor dem abbiegen halten
+    controller.setDesiredSpeedRight(0.5f);
+    controller.setDesiredSpeedLeft(-0.5f);
+    //wait(0.1f);
+    
+    
+    
     //Entscheiden welche Richtung, Drehen und Stoppen wenn die gewuenschte Anzahl Counts erreicht sind
     
     if (wallLeft == 0){ //Nach Links Drehen
@@ -40,12 +48,16 @@
         //printf("%d\n\r", countsLeft);
     }
     controller.setDesiredSpeedRight(0.5f);
-    controller.setDesiredSpeedLeft(0.5f);
+    controller.setDesiredSpeedLeft(-0.5f);
+    
+    dontStop = 1;
     
     }else if (wallFront == 0){ //Nicht Drehen-> weiter Geradeaus
     
         //printf("Vorne ist frei\n");
         
+        dontStop = 2;
+        
         }else if (wallRight == 0) { //Nach Rechts Drehen
             
             //printf("Rechts ist frei\n");
@@ -59,14 +71,16 @@
                 //printf("%d\n", countsLeft);
             }
             controller.setDesiredSpeedRight(0.5f);
-            controller.setDesiredSpeedLeft(0.5f);
+            controller.setDesiredSpeedLeft(-0.5f);
+            
+            dontStop = 3;
             
             
             }else{ //Alle Wege versperrt-> Wenden
             
                 //printf("Alles versperrt...zurueck\n");
             
-                while((countsRight <= countsRight0 + 2*TURNINGCOUNTS) && (countsLeft <= countsLeft0 + 2*TURNINGCOUNTS)){
+                while((countsRight <= countsRight0 + 2*TURNINGCOUNTS - 0) && (countsLeft <= countsLeft0 + 2*TURNINGCOUNTS - 0)){
                     controller.setDesiredSpeedRight(TURNINGSPEED);
                     controller.setDesiredSpeedLeft(TURNINGSPEED);
                     countsRight = counterRight.read();
@@ -75,6 +89,8 @@
                     //printf("%d\n", countsLeft);
                 }
                 controller.setDesiredSpeedRight(0.5f);  //0.0f
-                controller.setDesiredSpeedLeft(0.5f);   //0.0f
+                controller.setDesiredSpeedLeft(-0.5f);   //0.0f
+                
+                dontStop = 4;
             }
 }

--- a/Turn.h	Wed Apr 25 12:49:48 2018 +0000
+++ b/Turn.h	Mon May 07 18:52:04 2018 +0000
@@ -12,7 +12,7 @@
 
 public:
 
-    Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft);
+    Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft, int& dontStop);
     
     virtual ~Turn();
     void turning();
@@ -28,6 +28,7 @@
     int& wallRight;
     int& wallFront;
     int& wallLeft;
+    int& dontStop;
 };
 
 #endif /* TURN_H_ */
\ No newline at end of file

--- a/main.cpp	Wed Apr 25 12:49:48 2018 +0000
+++ b/main.cpp	Mon May 07 18:52:04 2018 +0000
@@ -9,8 +9,6 @@
 #include "CheckWalls.h"
 #include "Turn.h"
 
-
-
 //Initialisierung LEDs Blinker/Surri/Button
 DigitalIn button(USER_BUTTON);  //Moduswählknopf
 DigitalOut myled(LED1);     //Heartbeat (evt auch Anzeige für Modus, vor start
@@ -27,8 +25,6 @@
 PwmOut led5(PB_10);
 PwmOut led6(PB_9);
 
-
-
 //Initialisierung IR-Sensoren
 AnalogIn distance0(PC_2); //Kreieren der Eingangsobjekte
 AnalogIn distance1(PC_3);
@@ -39,22 +35,9 @@
 IRSensor irSensor2(distance2);    //links-vorne
 IRSensor irSensor3(distance3);    //links-hinten
 
-
-//AnalogIn distance(PB_1); //Kreieren der Ein - und Ausgangsobjekte
-//DigitalOut bit0(PH_1);
-//DigitalOut bit1(PC_2);
-//DigitalOut bit2(PC_3);
-//IRSensor irSensor0(distance, bit0, bit1, bit2, 0);
-//IRSensor irSensor1(distance, bit0, bit1, bit2, 1);
-//IRSensor irSensor2(distance, bit0, bit1, bit2, 2);
-//IRSensor irSensor3(distance, bit0, bit1, bit2, 3);
-
-
 //Initialisierung Kontrastsensor
 AnalogIn kontrast(PC_0);
 
-
-
 //Initialisierung Motor und Encoder
 DigitalOut enableMotorDriver(PB_2);
 PwmOut pwmLeft(PA_8);   //Motor links
@@ -78,13 +61,16 @@
     int wallFront = 0;
     int wallLeft = 0;
     int blackLine = 0;
+    int dontStop = 0;
+    //int way = 220;
     
 //    int way[5][10][4] = {{0},{0},{0}}; // Abspeicherung wird in roboterkoordinaten gespeichert vierstellige Zahl => 1.Zahl gefahrene Richtung, 2.Zahl Wand rechts, 3.Zahl Wand vorne, 4.Zahl Wand links
-
+    
+    //Funktionsdeklarationen
     CheckWalls checkWalls(irSensor0, irSensor1, irSensor2, wallRight, wallFront, wallLeft); //Ermittlung wo freie Wege(0) bzw welche Waende vorhanden (1)
-    Turn turn(counterLeft, counterRight, controller, wallRight, wallFront, wallLeft);   //Nach Ausrichtung bewegt sich der Roboter um ein Feld weiter und die Ausrichutung beginnt von vorne
+    Turn turn(counterLeft, counterRight, controller, wallRight, wallFront, wallLeft, dontStop);   //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, irSensor0, irSensor1, irSensor2, irSensor3); //20cm nach vorne...//Ablauf ist jeweils immer Feldweise...Waende checken, allenfalls drehen (Ausrichten), waehrend der Fahrt nur Ausrichtung der Geradeausfahrt...
+    Drive drive(kontrastSensor, counterLeft, counterRight, controller, irSensor0, irSensor1, irSensor2, irSensor3, dontStop); //20cm nach vorne...//Ablauf ist jeweils immer Feldweise...Waende checken, allenfalls drehen (Ausrichten), waehrend der Fahrt nur Ausrichtung der Geradeausfahrt...
 
 
     while(1) { // Wiederholungsschleife
@@ -96,6 +82,8 @@
 
                 while (button) {
                     //printf("WARTE AUF 1.START...");
+                    //controller.setDesiredSpeedRight(0.5f);
+                    //controller.setDesiredSpeedLeft(-0.5f);
                     wait(1.0f);         //Die Ruhe vor dem Sturm...
                 }
 
@@ -104,12 +92,9 @@
             }
             case 1: {// 1.Lauf Fahrzyklus und Aufzeichungszyklus
 
+
                 checkWalls.check(); //prueft wo Waende vorhanden sind
 
-                //wallRight = 1;
-                //wallFront = 1;
-                //wallLeft = 1;
-
                 turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
 
 
@@ -133,15 +118,12 @@
 //                        way[0][0][3] = 0;
 //                    }
                 //Abspeicherung des entschiedenen Wegs und Anpassung des
-
-
-//                    printf("%d", way[0][0][0]);
+                //dontStop = way%10;
+                //way = way/10;
+            
 
                 drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden
 
-
-
-                //blackLine = 1;
                 if(blackLine == 1) { //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier.
 
                     state = 2;
@@ -165,11 +147,16 @@
 
 
                 //pruefug der waender entfaellt, da jetzt die daten aus dem speicher geladen werden
-                checkWalls.check(); //prueft wo Waende vorhanden sind
+                //checkWalls.check(); //prueft wo Waende vorhanden sind
+                //
+                //integer fuer kuerzesten weg...1 rechts 2 gerade aus 3 links
+                // wallRight = 0;
+                // wallFront = 0;
+                // wallLeft = 0;
 
                 turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
 
-
+                //dontStop = 2;
 
                 drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden