Mapping.cpp

00001 #include "mbed.h"
00002 #include "Controller.h"
00003 #include "MotorDriver.h"
00004 #include "ReadFinalLine.h"
00005 #include "ReadSensor.h"
00006 #include "Mapping.h"
00007 //#include "SDFileSystem.h"
00008 //#include "FATFileSystem.h"
00009 
00010 DigitalOut myled1(LED1);
00011 
00012 // Funktionsinitalisierungen
00013 int readSensor(int SensorNummer);// Liesst Ultraschallsensoren 1-3 aus
00014 
00015 // int diveOne();  Bewegt Farhzeug um ein Feld nach vorne
00016 
00017 int turnRight(int currentDirection); // Biegt Farzeug nach rechts ab und gibt neue Momentanrichtung zurück
00018 
00019 int turnLeft(int currentDirection); // Biegt Farzeug nach links ab und gibt neue Momentanrichtung zurück
00020 
00021 //  int wait(int time); Systemfunktion für Pausen
00022 
00023 //SDFileSystem sd(PB_5, PB_4, PB_3, PB_10, "sd"); //mosi, miso, sclk, cs
00024 
00025 InterruptIn LineSens1(PC_2);
00026 InterruptIn LineSens2(PC_3);
00027 
00028 void mapping(int (*map)[10]){ //------------------------------------------------------------------------------------------------------ Mapping
00029 
00030 
00031 printf("Mappingvorgang wird gestartet...\n");
00032 int startPosX = 19; // Feld (10/5)
00033 int startPosY = 9;
00034 
00035 // inizialisieren der Karte
00036 //int map[20][10]; // 20 Zeilen, 10 Spalten, Karte gefüllt mit -1 für keine Daten
00037 int f,g;
00038 int filler = -1;
00039 for (f = 0; f < 20; f++) {
00040     for (g = 0; g < 10; g++) {
00041         map[f][g] = filler;
00042         }
00043     }
00044 
00045 int speed = 2;
00046 
00047 int startrichtung = 4;
00048 
00049 int P_straight;
00050 int P_rightTurn;
00051 int P_leftTurn;
00052 
00053 // Sensoren
00054 int right = 3;
00055 int left = 1;
00056 int front = 2;
00057 
00058 // Aktuelle Position
00059 int X = startPosX; // Bei Bewegung: X-2 -> 1 nach oben, X+2 -> 1 nach unten
00060 int Y = startPosY; // Bei Bewegung: Y-2 -> 1 nach links, Y+2 -> 1 nach rechts
00061 
00062 // Initalisieren der momentanen Richtung
00063 int currentDirection = startrichtung;
00064 
00065 // Verzögerung zwischen Aktionen in ms
00066 //int pause = 2;
00067 
00068 // Zustandsvariabel während Fahrt = 1 bei Ziel =0
00069 int drive = 1;
00070 
00071 // Startinitialisierung, Bestimmen der Abfahrtsrichtung aus dem ersten Feld
00072 
00073 if (readSensor(front) == false){    // Gegen vorne freie Fahrt (kein Hindernis -> 0)
00074     map[X-1][Y] = 0;
00075     }else{
00076         map[X-1][Y] = 1;
00077 }
00078 
00079 if (readSensor(right) == false){    // Gegen rechts freie Fahrt (kein Hindernis -> 0)
00080     map[X-1][Y-1] = 0;
00081     }else{
00082         map[X-1][Y-1] = 1;
00083 }
00084 
00085 map[X][Y-1] = 1; // ergibt sich aus Feldaufbau und Startrichtung
00086 map[X][Y] = 1;
00087 
00088 
00089 // Erster Schritt
00090 
00091 if(map[X-1][Y] == 0){
00092     driveOne(1,speed); 
00093     X = X-2;
00094 
00095 }
00096 else{
00097     currentDirection = turnRight(currentDirection);
00098     driveOne(1,speed);
00099     Y = Y-2;
00100 
00101 }
00102 //Interrupt für Ziellinie
00103 LineSens1.rise(&setLine1);
00104 LineSens2.rise(&setLine2);  
00105 
00106 
00107 // Farhralgorithmus
00108 
00109 while (drive == 1){ 
00110 
00111 
00112     // Variabeln zurücksetzen
00113      P_straight = 1;
00114      P_rightTurn = 1;
00115      P_leftTurn = 1;
00116 
00117 
00118  // Verhalten bei Schlaufenfahren = Bereits befahrenen Feldern
00119     
00120     // Setzt in Richtung des letzten Feldes eine Wand
00121     if (map[X][Y] != -1){
00122         
00123         switch (currentDirection){
00124             
00125             case 1:
00126             map[X][Y] = 1;
00127                 
00128             if (readSensor(front) == false){
00129             P_straight = 0;}
00130         
00131         
00132             //  Feld rechts
00133             if (readSensor(right) == false){
00134                 P_rightTurn = 0;}
00135         
00136         
00137             // Feld links
00138             if (readSensor(left) == false){
00139                 P_leftTurn = 0;}
00140             break;
00141             
00142             case 2:
00143                 map[X-1][Y] = 1;
00144                 //  Feld in Fahrtrichtung
00145         if (readSensor(front) == false){
00146             P_straight = 0;
00147         }
00148         
00149         
00150         //  Feld rechts
00151         if (readSensor(right) == false){
00152             P_rightTurn = 0;
00153         }
00154         
00155         //  Feld links
00156         
00157         if (readSensor(left) == false){
00158             P_leftTurn = 0;
00159         }
00160             break;
00161             
00162             case 3:
00163             map[X-1][Y-1] = 1;
00164             //  Feld in Fahrtrichtung
00165                 if (readSensor(front) == false){
00166                     P_straight = 0;
00167                 }
00168         
00169         
00170             //  Feld rechts
00171                 if (readSensor(right) == false){
00172                     P_rightTurn = 0;
00173                 }
00174       
00175         
00176             //  Feld links
00177         
00178                 if (readSensor(left) == false){
00179                     P_leftTurn = 0;
00180                 }        
00181             break;
00182             
00183             case 4:
00184                 map[X][Y-1] = 1;
00185                 
00186                 //  Feld in Fahrtrichtung
00187                   if (readSensor(front) == false){
00188                         P_straight = 0;
00189                     }
00190         
00191                 //  Feld rechts
00192                     if (readSensor(right) == false){
00193                         P_rightTurn = 0;
00194                     }
00195 
00196                 //  Feld links
00197                     if (readSensor(left) == false){
00198                         P_leftTurn = 0;
00199                     }
00200             break;
00201           
00202         }
00203         }else{
00204         
00205 
00206 
00207 
00208 
00209 // Mapping während Fahrt
00210     switch (currentDirection){
00211 // --------------------------------------------------------------------------- 1 Ausrichtung oben
00212 
00213 
00214         case 1:
00215             
00216             // Kartografierung Feld in Fahrtrichtung
00217         if (readSensor(front) == false){
00218             map[X-1][Y-1] = 0;
00219             P_straight = 0;
00220         }
00221         else{
00222             map[X-1][Y-1] = 1;
00223         }
00224         
00225         // Kartografierung Feld rechts
00226         if (readSensor(right) == false){
00227            map[X][Y-1] = 0;
00228             P_rightTurn = 0;
00229         }
00230         else{
00231             map[X][Y-1] = 1;
00232         }
00233         
00234         // Kartografierung Feld links
00235         
00236         if (readSensor(left) == false){
00237             map[X-1][Y] = 0;
00238             P_leftTurn = 0;
00239         }
00240         else{
00241             map[X-1][Y] = 1;
00242         }
00243         
00244         // Kartografierung Feld nach hinten
00245         map[X][Y] = 1;
00246         break;
00247 
00248 // --------------------------------------------------------------------------- 2 Ausrichtung Rechts
00249         case 2:
00250             
00251             // Kartografierung Feld in Fahrtrichtung
00252         if (readSensor(front) == false){
00253             map[X][Y-1] = 0;
00254             P_straight = 0;
00255         }
00256         else{
00257             map[X][Y-1] = 1;
00258         }
00259         
00260         // Kartografierung Feld rechts
00261         if (readSensor(right) == false){
00262             map[X][Y] = 0;
00263             P_rightTurn = 0;
00264         }
00265         else{
00266             map[X][Y] = 1;
00267         }
00268         
00269         // Kartografierung Feld links
00270         
00271         if (readSensor(left) == false){
00272             map[X-1][Y-1] = 0;
00273             P_leftTurn = 0;
00274         }
00275         else{
00276             map[X-1][Y-1] = 1;
00277         }
00278         
00279         // Kartografierung Feld nach hinten
00280         map[X-1][Y] = 1;
00281         break;
00282 // --------------------------------------------------------------------------- 3 Ausrichtung Unten
00283         case 3:
00284             
00285             // Kartografierung Feld in Fahrtrichtung
00286         if (readSensor(front) == false){
00287             map[X][Y] = 0;
00288             P_straight = 0;
00289         }
00290         else{
00291             map[X][Y] = 1;
00292         }
00293         
00294         // Kartografierung Feld rechts
00295         if (readSensor(right) == false){
00296             map[X-1][Y] = 0;
00297             P_rightTurn = 0;
00298         }
00299         else{
00300             map[X-1][Y] = 1;
00301         }
00302         
00303         // Kartografierung Feld links
00304         
00305         if (readSensor(left) == false){
00306             map[X][Y-1] = 0;
00307             P_leftTurn = 0;
00308         }
00309         else{
00310             map[X][Y-1] = 1;
00311         }
00312         
00313         // Kartografierung Feld nach hinten
00314         map[X-1][Y-1] = 1;
00315         break;
00316 // --------------------------------------------------------------------------- 4 Ausrichtung Links
00317     case 4:
00318             
00319             // Kartografierung Feld in Fahrtrichtung
00320         if (readSensor(front) == false){
00321            map[X-1][Y] = 0;
00322             P_straight = 0;
00323         }
00324         else{
00325            map[X-1][Y] = 1;
00326         }
00327         
00328         // Kartografierung Feld rechts
00329         if (readSensor(right) == false){
00330             map[X-1][Y-1] = 0;
00331             P_rightTurn = 0;
00332         }
00333         else{
00334             map[X-1][Y-1] = 1;
00335         }
00336         
00337         // Kartografierung Feld links
00338         
00339         if (readSensor(left) == false){
00340             map[X][Y] = 0;
00341             P_leftTurn = 0;
00342         }
00343         else{
00344             map[X][Y] = 1;
00345         }
00346         
00347         // Kartografierung Feld nach hinten
00348         map[X][Y-1] = 1;
00349 
00350         break;
00351     default: printf("Error: Direction out of definition\n");
00352     }
00353   }      
00354     //printf("Gerade %d\nRechts %d\nLinks %d\n", P_straight, P_rightTurn, P_leftTurn );
00355       
00356     //printf("Momentane Richtung = %d\n", currentDirection );
00357 
00358 
00359     // Fahren gem. Rechtsfahralgorithmus
00360     if (P_straight ==1){
00361         driveOne(1,1);
00362         }
00363 
00364     if (P_rightTurn == 0) {
00365         currentDirection = turnRight(currentDirection);
00366         driveOne(1,speed);
00367         } else{
00368             if (P_straight == 0){
00369                 driveOne(1,speed); 
00370             } else{
00371                 if(P_leftTurn == 0){
00372                     currentDirection = turnLeft(currentDirection);
00373                     driveOne(1,speed);
00374                     } else{
00375                         currentDirection = turnRight(currentDirection);
00376                         currentDirection = turnRight(currentDirection);
00377                         driveOne(1,speed);
00378                         }
00379             }
00380         }
00381         
00382         
00383         
00384     // Aktualisieren der Position:  
00385     
00386     switch (currentDirection){
00387         case 1:
00388         Y = Y-2;    
00389         break;
00390         //----------        
00391         case 2:
00392         X = X+2;    
00393         break;  
00394         //----------    
00395         case 3:
00396         Y = Y+2;    
00397         break;  
00398         //----------
00399         case 4:
00400         X = X-2;
00401         break;  
00402     }
00403     
00404     // Bei Ueberfahren der Ziellinie
00405     if(readFinalLine() == true){ // Prüft Linienüberfahrt
00406         drive = 0; // Beendet Schlafendurchlauf
00407     }
00408     
00409     //printf("\nMap:\n %d, %d;\n     %d, %d;",map[X-1][Y-1],map[X][Y-1],map[X-1][Y],map[X][Y]);
00410     
00411     } // Ende Fahrschlaufe
00412 
00413 // Zielmanoever ausführen-------------------------------------------------------------------------------------
00414 
00415 map[X][Y] = 100;
00416 map[X-1][Y] = 100;
00417 map[X][Y-1] = 100;
00418 map[X-1][Y-1] = 100;
00419  
00420  int k = 0;
00421  int i = 0;
00422  
00423         for (int i = 0; i <= 9; i++) {
00424             for (int k = 0; k <= 19; k++){
00425                 printf("%d;",map[k][i]); 
00426             }
00427             printf("\n");
00428             }
00429  }
00430