Diff: Mapping.cpp

Revision:

0:4a0b987c5c94

Child:

6:a1fd0f1374e6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Mapping.cpp	Fri Apr 27 12:30:38 2018 +0000
@@ -0,0 +1,409 @@
+#include "mbed.h"
+#include "Controller.h"
+#include "MotorDriver.h"
+#include "ReadFinalLine.h"
+#include "ReadSensor.h"
+#include "Mapping.h"
+
+//debug
+//DigitalOut myled(LED2);
+
+// Funktionsinitalisierungen
+int readSensor(int SensorNummer);// Liesst Ultraschallsensoren 1-3 aus
+
+// int diveOne();  Bewegt Farhzeug um ein Feld nach vorne
+
+int turnRight(int currentDirection); // Biegt Farzeug nach rechts ab und gibt neue Momentanrichtung zurück
+
+int turnLeft(int currentDirection); // Biegt Farzeug nach links ab und gibt neue Momentanrichtung zurück
+
+//  int wait(int time); Systemfunktion für Pausen
+
+// SDFileSystem sd(PB_5, PB_4, PB_3, PB_10, "sd"); //mosi, miso, sclk, cs
+
+void mapping(void){ //------------------------------------------------------------------------------------------------------ Mapping
+
+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 speed = 1;
+
+int startrichtung = 4;
+
+int P_straight;
+int P_rightTurn;
+int P_leftTurn;
+
+// Sensoren
+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
+
+// Initalisieren der momentanen Richtung
+int currentDirection = startrichtung;
+
+// Verzögerung zwischen Aktionen in ms
+int pause = 2;
+
+// Zustandsvariabel während Fahrt = 1 bei Ziel =0
+int drive = 1;
+
+int targetReached = 0; // Wird auf 1 gesetzt bei überschrittener Ziellinie sonst 0
+
+// 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{
+        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][ 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;
+        // debug
+    printf("\nIf erster schritt\n");
+}
+else{
+    currentDirection = turnRight(currentDirection);
+    wait(pause);
+    driveOne(1,speed);
+    Y = Y-2;
+    printf("\nElse erster schritt\n");
+}
+
+
+// Farhralgorithmus
+
+while (drive == 1){
+wait(pause);    
+    // Variabeln zurücksetzen
+     P_straight = 1;
+     P_rightTurn = 1;
+     P_leftTurn = 1;
+
+    int s = 12;
+
+// Mapping während Fahrt
+    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;
+
+// --------------------------------------------------------------------------- 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;
+// --------------------------------------------------------------------------- 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;
+        }
+        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");
+    }
+    // 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 );
+
+
+    // Fahren gem. Rechtsfahralgorithmus
+
+    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){
+        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(targetReached == 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){
+            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;
+
+// Karte auf SD speichern-------------------------------------------------------------------------------------
+
+ /*
+//Mount the filesystem
+
+    sd.mount();
+    int i;
+    int g;
+
+    //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; g < 19; g++){
+            fprintf(fp, "%d,",map[g,i]);
+            }
+            fprintf(fp,"\n\r")
+        }
+        printf("Matrix has been sucessfully saved in map.csv\n\r");
+    } else {
+        printf("Save-process failed!\n\r");
+    }
+
+    //Unmount the filesystem
+    sd.unmount();
+    */
+}
+
+
+
+
+
+
+
+
+
+
+
+