Diff: Mapping.cpp

Revision:

12:811b1364679e

Parent:

11:68ee67d17320

--- a/Mapping.cpp	Sat May 12 12:19:26 2018 +0000
+++ b/Mapping.cpp	Fri May 18 11:36:48 2018 +0000
@@ -26,383 +26,360 @@
 InterruptIn LineSens1(PC_2);
 InterruptIn LineSens2(PC_3);
 
-void mapping(void){ //------------------------------------------------------------------------------------------------------ Mapping
+void mapping(void)  //------------------------------------------------------------------------------------------------------ Mapping
+{
 
-int startPosX = 19; // Feld (10 / 5)
-int startPosY = 9;
+    int startPosX = 19; // Feld (10 / 5)
+    int startPosY = 9;
 
 
-int map[20][10]; // 20 Zeilen, 10 Spalten, Karte gefüllt mit -1 für keine Daten
-int f,g;
-for (f = 0; f < 3; f++) {
-for (g = 0; g < 5; g++) {
-map[f][g] = -1;
-}
-}
+    int map[20][10]; // 20 Zeilen, 10 Spalten, Karte gefüllt mit 3 für keine Daten
+    int f,g;
+    for (f = 0; f < 20; f++) {
+        for (g = 0; g < 10; g++) {
+            map[f][g] = 3;
+        }
+    }
 
-int speed = 3;
+    int speed = 3;
 
-int startrichtung = 4;
+    int startrichtung = 4;
 
-int P_straight;
-int P_rightTurn;
-int P_leftTurn;
+    int P_straight;
+    int P_rightTurn;
+    int P_leftTurn;
 
 // Sensoren
-int right = 3;
-int left = 1;
-int front = 2;
+    int right = 3;
+    int left = 1;
+    int front = 2;
 
 // Aktuelle Position
-int X = startPosX; // Bei Bewegung: X-2 -> 1 nach oben, X+2 -> 1 nach unten
-int Y = startPosY; // Bei Bewegung: Y-2 -> 1 nach links, Y+2 -> 1 nach rechts
+    int X = startPosX; // Bei Bewegung: X-2 -> 1 nach oben, X+2 -> 1 nach unten
+    int Y = startPosY; // Bei Bewegung: Y-2 -> 1 nach links, Y+2 -> 1 nach rechts
 
 // Initalisieren der momentanen Richtung
-int currentDirection = startrichtung;
+    int currentDirection = startrichtung;
 
 // Verzögerung zwischen Aktionen in ms
-int pause = 2;
+    int pause = 2;
 
 // Zustandsvariabel während Fahrt = 1 bei Ziel =0
-int drive = 1;
+    int drive = 1;
 
 // Startinitialisierung, Bestimmen der Abfahrtsrichtung aus dem ersten Feld
 
-if (readSensor(front) == false){    // Gegen vorne freie Fahrt (kein Hindernis -> 0)
-    map[X-1][Y] = 0;
-    }else{
+    if (readSensor(front) == false) {   // Gegen vorne freie Fahrt (kein Hindernis -> 0)
+        map[X-1][Y] = 0;
+    } else {
         map [X-1][Y] = 1;
-}
+    }
 
-if (readSensor(right) == false){    // Gegen rechts freie Fahrt (kein Hindernis -> 0)
-    map[X-1][Y-1] = 0;
-    }else{
+    if (readSensor(right) == false) {   // Gegen rechts freie Fahrt (kein Hindernis -> 0)
+        map[X-1][Y-1] = 0;
+    } else {
         map [X-1][Y-1] = 1;
-}
+    }
 
-map[X][ Y-1] = 1; // ergibt sich aus Feldaufbau und Startrichtung
-map[X ][ Y] = 1;
+    map[X][ Y-1] = 1; // ergibt sich aus Feldaufbau und Startrichtung
+    map[X ][ Y] = 1;
 
 
 // Erster Schritt
 
-if(map[X-1][Y] == 0){
-    driveOne(1,speed); 
-    X = X-2;
+    if(map[X-1][Y] == 0) {
+        driveOne(1,speed);
+        X = X-2;
         // debug
-    //printf("\nIf erster schritt\n");
-}
-else{
-    currentDirection = turnRight(currentDirection);
-    //wait(pause);
-    driveOne(1,speed);
-    Y = Y-2;
-    //printf("\nElse erster schritt\n");
-}
+        //printf("\nIf erster schritt\n");
+    } else {
+        currentDirection = turnRight(currentDirection);
+        //wait(pause);
+        driveOne(1,speed);
+        Y = Y-2;
+        //printf("\nElse erster schritt\n");
+    }
 //Interrupt für Ziellinie
-LineSens1.rise(&setLine1);
-LineSens2.rise(&setLine2);  
-    
+    LineSens1.rise(&setLine1);
+    LineSens2.rise(&setLine2);
+
 
 // Farhralgorithmus
 
-while (drive == 1){
-//wait(pause);  
+    while (drive == 1) {
+//wait(pause);
 
-    // Variabeln zurücksetzen
-     P_straight = 1;
-     P_rightTurn = 1;
-     P_leftTurn = 1;
+        // Variabeln zurücksetzen
+        P_straight = 1;
+        P_rightTurn = 1;
+        P_leftTurn = 1;
 
-    int s = 12;
+        int s = 12;
 
 // Mapping während Fahrt
-    switch (currentDirection){
+        switch (currentDirection) {
 // --------------------------------------------------------------------------- 1 Ausrichtung oben
 
 
-        case 1:
-            
-            // Kartografierung Feld in Fahrtrichtung
-        if (readSensor(front) == false){
-            map[X-1][Y-1] = 0;
-            P_straight = 0;
-        }
-        else{
-            map[X-1][Y-1] = 1;
-        }
-        
-        // Kartografierung Feld rechts
-        if (readSensor(right) == false){
-            map[X][Y-1] = 0;
-            P_rightTurn = 0;
-        }
-        else{
-            map[X][Y-1] = 1;
-        }
-        
-        // Kartografierung Feld links
-        
-        if (readSensor(left) == false){
-            map[X-1][Y] = 0;
-            P_leftTurn = 0;
-        }
-        else{
-            map[X-1][Y] = 1;
-        }
-        
-        // Kartografierung Feld nach hinten
-        map[X][Y] = 1;
-            // debug
-    //printf("case 1 \n");
-        break;
+            case 1:
+
+                // Kartografierung Feld in Fahrtrichtung
+                if (readSensor(front) == false) {
+                    map[X-1][Y-1] = 0;
+                    P_straight = 0;
+                } else {
+                    map[X-1][Y-1] = 1;
+                }
+
+                // Kartografierung Feld rechts
+                if (readSensor(right) == false) {
+                    map[X][Y-1] = 0;
+                    P_rightTurn = 0;
+                } else {
+                    map[X][Y-1] = 1;
+                }
+
+                // Kartografierung Feld links
+
+                if (readSensor(left) == false) {
+                    map[X-1][Y] = 0;
+                    P_leftTurn = 0;
+                } else {
+                    map[X-1][Y] = 1;
+                }
+
+                // Kartografierung Feld nach hinten
+                map[X][Y] = 1;
+                // debug
+                //printf("case 1 \n");
+                break;
 
 // --------------------------------------------------------------------------- 2 Ausrichtung Rechts
-        case 2:
-            
-            // Kartografierung Feld in Fahrtrichtung
-        if (readSensor(front) == false){
-            map[X][Y-1] = 0;
-            P_straight = 0;
-        }
-        else{
-            map[X][Y-1] = 1;
-        }
-        
-        // Kartografierung Feld rechts
-        if (readSensor(right) == false){
-            map[X][Y] = 0;
-            P_rightTurn = 0;
-        }
-        else{
-            map[X][Y] = 1;
-        }
-        
-        // Kartografierung Feld links
-        
-        if (readSensor(left) == false){
-            map[X-1][Y-1] = 0;
-            P_leftTurn = 0;
-        }
-        else{
-            map[X-1][Y-1] = 1;
-        }
-        
-        // Kartografierung Feld nach hinten
-        map[X-1][Y] = 1;
-            // debug
-    //printf("case 2 \n");
-        break;
+            case 2:
+
+                // Kartografierung Feld in Fahrtrichtung
+                if (readSensor(front) == false) {
+                    map[X][Y-1] = 0;
+                    P_straight = 0;
+                } else {
+                    map[X][Y-1] = 1;
+                }
+
+                // Kartografierung Feld rechts
+                if (readSensor(right) == false) {
+                    map[X][Y] = 0;
+                    P_rightTurn = 0;
+                } else {
+                    map[X][Y] = 1;
+                }
+
+                // Kartografierung Feld links
+
+                if (readSensor(left) == false) {
+                    map[X-1][Y-1] = 0;
+                    P_leftTurn = 0;
+                } else {
+                    map[X-1][Y-1] = 1;
+                }
+
+                // Kartografierung Feld nach hinten
+                map[X-1][Y] = 1;
+                // debug
+                //printf("case 2 \n");
+                break;
 // --------------------------------------------------------------------------- 3 Ausrichtung Unten
-        case 3:
-            
-            // Kartografierung Feld in Fahrtrichtung
-        if (readSensor(front) == false){
-            map[X][Y] = 0;
-            P_straight = 0;
-        }
-        else{
-            map[X][Y] = 1;
-        }
-        
-        // Kartografierung Feld rechts
-        if (readSensor(right) == false){
-            map[X-1][Y] = 0;
-            P_rightTurn = 0;
-        }
-        else{
-            map[X-1][Y] = 1;
-        }
-        
-        // Kartografierung Feld links
-        
-        if (readSensor(left) == false){
-            map[X][Y-1] = 0;
-            P_leftTurn = 0;
+            case 3:
+
+                // Kartografierung Feld in Fahrtrichtung
+                if (readSensor(front) == false) {
+                    map[X][Y] = 0;
+                    P_straight = 0;
+                } else {
+                    map[X][Y] = 1;
+                }
+
+                // Kartografierung Feld rechts
+                if (readSensor(right) == false) {
+                    map[X-1][Y] = 0;
+                    P_rightTurn = 0;
+                } else {
+                    map[X-1][Y] = 1;
+                }
+
+                // Kartografierung Feld links
+
+                if (readSensor(left) == false) {
+                    map[X][Y-1] = 0;
+                    P_leftTurn = 0;
+                } else {
+                    map[X][Y-1] = 1;
+                }
+
+                // Kartografierung Feld nach hinten
+                map[X-1][Y-1] = 1;
+                // debug
+                //printf("case 3: S = %d\n",s);
+                break;
+// --------------------------------------------------------------------------- 4 Ausrichtung Links
+            case 4:
+
+                // Kartografierung Feld in Fahrtrichtung
+                if (readSensor(front) == false) {
+                    map[X-1][Y] = 0;
+                    P_straight = 0;
+                } else {
+                    map[X-1][Y] = 1;
+                }
+
+                // Kartografierung Feld rechts
+                if (readSensor(right) == false) {
+                    map[X-1][Y-1] = 0;
+                    P_rightTurn = 0;
+                } else {
+                    map[X-1][Y-1] = 1;
+                }
+
+                // Kartografierung Feld links
+
+                if (readSensor(left) == false) {
+                    map[X][Y] = 0;
+                    P_leftTurn = 0;
+                } else {
+                    map[X][Y] = 1;
+                }
+
+                // Kartografierung Feld nach hinten
+                map[X-1][Y-1] = 1;
+                // debug
+                //printf("case 4\n");
+
+                break;
+            default: //printf("Error: Direction out of definition\n");
         }
-        else{
-            map[X][Y-1] = 1;
+        // debug
+        if (P_straight == 0) {
+            //printf("Geradeaus frei");
         }
-        
-        // Kartografierung Feld nach hinten
-        map[X-1][Y-1] = 1;
-            // debug
-    //printf("case 3: S = %d\n",s);
-        break;
-// --------------------------------------------------------------------------- 4 Ausrichtung Links
-    case 4:
-            
-            // Kartografierung Feld in Fahrtrichtung
-        if (readSensor(front) == false){
-            map[X-1][Y] = 0;
-            P_straight = 0;
-        }
-        else{
-            map[X-1][Y] = 1;
-        }
-        
-        // Kartografierung Feld rechts
-        if (readSensor(right) == false){
-            map[X-1][Y-1] = 0;
-            P_rightTurn = 0;
-        }
-        else{
-            map[X-1][Y-1] = 1;
+        if (P_rightTurn == 0) {
+            //printf("Rechts frei");
         }
-        
-        // Kartografierung Feld links
-        
-        if (readSensor(left) == false){
-            map[X][Y] = 0;
-            P_leftTurn = 0;
+        if (P_leftTurn == 0) {
+            //printf("Links frei");
         }
-        else{
-            map[X][Y] = 1;
-        }
-        
-        // Kartografierung Feld nach hinten
-        map[X-1][Y-1] = 1;
-         // debug
-    //printf("case 4\n");
+        //printf("Gerade %d\nRechts %d\nLinks %d\n", P_straight, P_rightTurn, P_leftTurn );
 
-        break;
-    default: //printf("Error: Direction out of definition\n");
-    }
-    // debug
-    if (P_straight == 0){
-        //printf("Geradeaus frei");
-        }
-         if (P_rightTurn == 0){
-        //printf("Rechts frei");
-        }
-    if (P_leftTurn == 0){
-        //printf("Links frei");
-        }    
-    //printf("Gerade %d\nRechts %d\nLinks %d\n", P_straight, P_rightTurn, P_leftTurn );
-      
-    //printf("Momentane Richtung = %d\n", currentDirection );
+        //printf("Momentane Richtung = %d\n", currentDirection );
 
 
-    // Fahren gem. Rechtsfahralgorithmus
-    if (P_straight ==1){
-        driveOne(1,1);
+        // Fahren gem. Rechtsfahralgorithmus
+        if (P_straight ==1) {
+            driveOne(1,1);
         }
 
-    if (P_rightTurn == 0) {
-        currentDirection = turnRight(currentDirection);
-        //wait(pause);
-        //printf("Ich biege rechts ab\n");
-        driveOne(1,speed);
-        //myled = 0;
-        } else{
-            if (P_straight == 0){
-                driveOne(1,speed); 
-                 //printf("Ich fahre gerade aus\n");
+        if (P_rightTurn == 0) {
+            currentDirection = turnRight(currentDirection);
+            //wait(pause);
+            //printf("Ich biege rechts ab\n");
+            driveOne(1,speed);
+            //myled = 0;
+        } else {
+            if (P_straight == 0) {
+                driveOne(1,speed);
+                //printf("Ich fahre gerade aus\n");
                 //myled = 0;
-            } else{
-                if(P_leftTurn == 0){
+            } else {
+                if(P_leftTurn == 0) {
                     currentDirection = turnLeft(currentDirection);
                     //wait(pause);
-                     //printf("Ich biege links ab\n");
+                    //printf("Ich biege links ab\n");
                     driveOne(1,speed);
                     //myled = 0;
-                    } else{
-                         //printf("Ich wende!!!\n");
-                        currentDirection = turnRight(currentDirection);
-                        //wait(pause);
-                        currentDirection = turnRight(currentDirection);
-                        //wait(pause);
-                        driveOne(1,speed);
-                        //myled = 1;
-                        }
+                } else {
+                    //printf("Ich wende!!!\n");
+                    currentDirection = turnRight(currentDirection);
+                    //wait(pause);
+                    currentDirection = turnRight(currentDirection);
+                    //wait(pause);
+                    driveOne(1,speed);
+                    //myled = 1;
+                }
             }
         }
-        
-       //printf("Neue Richtung = %d\n", currentDirection ); 
-    // Aktualisieren der Position:  
-    
-    switch (currentDirection){
-        case 1:
-        Y = Y-2;    
-        break;
-        //----------        
-        case 2:
-        X = X+2;    
-        break;  
-        //----------    
-        case 3:
-        Y = Y+2;    
-        break;  
-        //----------
-        case 4:
-        X = X-2;
-        break;  
-    }
-    
-    // Bei Ueberfahren der Ziellinie
-    if(readFinalLine() == true){ // Prüft Linienüberfahrt
-        drive = 0; // Beendet Schlafendurchlauf
-    }
-    
-    // Verhalten bei Schlaufenfahren = Bereits befahrenen Feldern
-    
-    // Setzt in Richtung des letzten Feldes eine Wand
-    if (map[X][Y] != -1){
-        switch (currentDirection){
+
+        //printf("Neue Richtung = %d\n", currentDirection );
+        // Aktualisieren der Position:
+
+        switch (currentDirection) {
             case 1:
-                map[X][Y] = 1;
-            break;
-            
+                Y = Y-2;
+                break;
+                //----------
             case 2:
-                map[X-1][Y] = 1;
-            break;
-            
+                X = X+2;
+                break;
+                //----------
             case 3:
-                map[X-1][Y-1] = 1;
-            break;
-            
+                Y = Y+2;
+                break;
+                //----------
             case 4:
-                map[X][Y-1] = 1;
-            break;
+                X = X-2;
+                break;
+        }
+
+        // Bei Ueberfahren der Ziellinie
+        if(readFinalLine() == true) { // Prüft Linienüberfahrt
+            drive = 0; // Beendet Schlafendurchlauf
         }
-        
-    }
-    
+
+        // Verhalten bei Schlaufenfahren = Bereits befahrenen Feldern
+
+        // Setzt in Richtung des letzten Feldes eine Wand
+        if (map[X][Y] != 3) {
+            switch (currentDirection) {
+                case 1:
+                    map[X][Y] = 1;
+                    break;
+
+                case 2:
+                    map[X-1][Y] = 1;
+                    break;
+
+                case 3:
+                    map[X-1][Y-1] = 1;
+                    break;
+
+                case 4:
+                    map[X][Y-1] = 1;
+                    break;
+            }
+
+        }
+
     } // Ende Fahrschlaufe
 
 // Zielmanoever ausführen-------------------------------------------------------------------------------------
 
-map[X][Y] = 100;
-map[X-1][Y] = 100;
-map[X][Y-1] = 100;
-map[X-1][Y-1] = 100;
+    map[X][Y] = 2;
+    map[X-1][Y] = 2;
+    map[X][Y-1] = 2;
+    map[X-1][Y-1] = 2;
 
 // Karte auf SD speichern-------------------------------------------------------------------------------------
 
- 
-//Mount the filesystem
-
-    sd.mount();
-    int i;
-    int k;
-
-    //Perform a write test
-    //printf("\nWriting to SD card...");
     FILE *fp = fopen("/sd/map.csv", "w");
     if (fp != NULL) {
-        for (i = 0; i < 9; i++){
-            for (g = 0; k < 19; g++){
-            fprintf(fp, "%d,",map[k,i]);
+        for (int i = 0; i <= 9; i++) {
+            for (int k = 0; k <= 19; g++) {
+                fprintf(fp, "%d",map[k,i]); //fprintf(fp, "Hello fun SD Card World!");
             }
-            fprintf(fp,"\n\r");
+            fprintf(fp,"\n");
         }
-        //printf("Matrix has been sucessfully saved in map.csv\n\r");
     } else {
-        //printf("Save-process failed!\n\r");
+        printf("Zugriff auf Karte verweigert");
     }
+    fclose(fp);
 
-    //Unmount the filesystem
-    sd.unmount();
     while(1) {
         myled1 = 1;
         wait(0.2);