Diff: Mapping.cpp

Revision:

14:0caa7b93af7a

Parent:

9:5802ccfcc984

--- a/Mapping.cpp	Tue May 08 12:32:31 2018 +0000
+++ b/Mapping.cpp	Wed May 23 11:50:58 2018 +0000
@@ -4,13 +4,11 @@
 #include "ReadFinalLine.h"
 #include "ReadSensor.h"
 #include "Mapping.h"
-#include "SDFileSystem.h"
+//#include "SDFileSystem.h"
+//#include "FATFileSystem.h"
 
 DigitalOut myled1(LED1);
 
-//debug
-//DigitalOut myled(LED2);
-
 // Funktionsinitalisierungen
 int readSensor(int SensorNummer);// Liesst Ultraschallsensoren 1-3 aus
 
@@ -22,23 +20,27 @@
 
 //  int wait(int time); Systemfunktion für Pausen
 
-SDFileSystem sd(PB_5, PB_4, PB_3, PB_10, "sd"); //mosi, miso, sclk, cs
+//SDFileSystem sd(PB_5, PB_4, PB_3, PB_10, "sd"); //mosi, miso, sclk, cs
+
 InterruptIn LineSens1(PC_2);
 InterruptIn LineSens2(PC_3);
 
-void mapping(void){ //------------------------------------------------------------------------------------------------------ Mapping
-
-int startPosX = 19; // Feld (10 / 5)
-int startPosY = 9;
+void mapping(int (*map)[10]){ //------------------------------------------------------------------------------------------------------ Mapping
 
 
-int map[20][10]; // 20 Zeilen, 10 Spalten, Karte gefüllt mit -1 für keine Daten
+printf("Mappingvorgang wird gestartet...\n");
+int startPosX = 19; // Feld (10/5)
+int startPosY = 9;
+
+// inizialisieren der Karte
+//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 filler = -1;
+for (f = 0; f < 20; f++) {
+    for (g = 0; g < 10; g++) {
+        map[f][g] = filler;
+        }
+    }
 
 int speed = 2;
 
@@ -61,7 +63,7 @@
 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;
@@ -71,17 +73,17 @@
 if (readSensor(front) == false){    // Gegen vorne freie Fahrt (kein Hindernis -> 0)
     map[X-1][Y] = 0;
     }else{
-        map [X-1][Y] = 1;
+        map[X-1][Y] = 1;
 }
 
 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-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
@@ -89,32 +91,120 @@
 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");
+
 }
 //Interrupt für Ziellinie
 LineSens1.rise(&setLine1);
 LineSens2.rise(&setLine2);  
-    
+
 
 // Farhralgorithmus
 
-while (drive == 1){
-//wait(pause);  
+while (drive == 1){ 
+
 
     // Variabeln zurücksetzen
      P_straight = 1;
      P_rightTurn = 1;
      P_leftTurn = 1;
 
-    int s = 12;
+
+ // Verhalten bei Schlaufenfahren = Bereits befahrenen Feldern
+    
+    // Setzt in Richtung des letzten Feldes eine Wand
+    if (map[X][Y] != -1){
+        
+        switch (currentDirection){
+            
+            case 1:
+            map[X][Y] = 1;
+                
+            if (readSensor(front) == false){
+            P_straight = 0;}
+        
+        
+            //  Feld rechts
+            if (readSensor(right) == false){
+                P_rightTurn = 0;}
+        
+        
+            // Feld links
+            if (readSensor(left) == false){
+                P_leftTurn = 0;}
+            break;
+            
+            case 2:
+                map[X-1][Y] = 1;
+                //  Feld in Fahrtrichtung
+        if (readSensor(front) == false){
+            P_straight = 0;
+        }
+        
+        
+        //  Feld rechts
+        if (readSensor(right) == false){
+            P_rightTurn = 0;
+        }
+        
+        //  Feld links
+        
+        if (readSensor(left) == false){
+            P_leftTurn = 0;
+        }
+            break;
+            
+            case 3:
+            map[X-1][Y-1] = 1;
+            //  Feld in Fahrtrichtung
+                if (readSensor(front) == false){
+                    P_straight = 0;
+                }
+        
+        
+            //  Feld rechts
+                if (readSensor(right) == false){
+                    P_rightTurn = 0;
+                }
+      
+        
+            //  Feld links
+        
+                if (readSensor(left) == false){
+                    P_leftTurn = 0;
+                }        
+            break;
+            
+            case 4:
+                map[X][Y-1] = 1;
+                
+                //  Feld in Fahrtrichtung
+                  if (readSensor(front) == false){
+                        P_straight = 0;
+                    }
+        
+                //  Feld rechts
+                    if (readSensor(right) == false){
+                        P_rightTurn = 0;
+                    }
+
+                //  Feld links
+                    if (readSensor(left) == false){
+                        P_leftTurn = 0;
+                    }
+            break;
+          
+        }
+        }else{
+        
+
+
+
 
 // Mapping während Fahrt
     switch (currentDirection){
@@ -134,7 +224,7 @@
         
         // Kartografierung Feld rechts
         if (readSensor(right) == false){
-            map[X][Y-1] = 0;
+           map[X][Y-1] = 0;
             P_rightTurn = 0;
         }
         else{
@@ -153,8 +243,6 @@
         
         // Kartografierung Feld nach hinten
         map[X][Y] = 1;
-            // debug
-    //printf("case 1 \n");
         break;
 
 // --------------------------------------------------------------------------- 2 Ausrichtung Rechts
@@ -190,8 +278,6 @@
         
         // Kartografierung Feld nach hinten
         map[X-1][Y] = 1;
-            // debug
-    //printf("case 2 \n");
         break;
 // --------------------------------------------------------------------------- 3 Ausrichtung Unten
         case 3:
@@ -226,19 +312,17 @@
         
         // 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;
+           map[X-1][Y] = 0;
             P_straight = 0;
         }
         else{
-            map[X-1][Y] = 1;
+           map[X-1][Y] = 1;
         }
         
         // Kartografierung Feld rechts
@@ -261,23 +345,12 @@
         }
         
         // Kartografierung Feld nach hinten
-        map[X-1][Y-1] = 1;
-         // debug
-    //printf("case 4\n");
+        map[X][Y-1] = 1;
 
         break;
-    default: //printf("Error: Direction out of definition\n");
+    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 );
@@ -290,35 +363,24 @@
 
     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){
                     currentDirection = turnLeft(currentDirection);
-                    //wait(pause);
-                     //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;
                         }
             }
         }
         
-       //printf("Neue Richtung = %d\n", currentDirection ); 
+        
+        
     // Aktualisieren der Position:  
     
     switch (currentDirection){
@@ -344,29 +406,7 @@
         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){
-            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;
-        }
-        
-    }
+    //printf("\nMap:\n %d, %d;\n     %d, %d;",map[X-1][Y-1],map[X][Y-1],map[X-1][Y],map[X][Y]);
     
     } // Ende Fahrschlaufe
 
@@ -376,49 +416,15 @@
 map[X-1][Y] = 100;
 map[X][Y-1] = 100;
 map[X-1][Y-1] = 100;
-
-// Karte auf SD speichern-------------------------------------------------------------------------------------
-
+ 
+ int k = 0;
+ int i = 0;
  
-//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; k++){
+                printf("%d;",map[k][i]); 
             }
-            fprintf(fp,"\n\r");
-        }
-        //printf("Matrix has been sucessfully saved in map.csv\n\r");
-    } else {
-        //printf("Save-process failed!\n\r");
-    }
+            printf("\n");
+            }
+ }
 
-    //Unmount the filesystem
-    sd.unmount();
-    while(1) {
-        myled1 = 1;
-        wait(0.2);
-        myled1 = 0;
-        wait(0.2);
-    }
-}
-
-
-
-
-
-
-
-
-
-
-
-