File content as of revision 12:811b1364679e:

#include "mbed.h"
#include "Controller.h"
#include "MotorDriver.h"
#include "ReadFinalLine.h"
#include "ReadSensor.h"
#include "Mapping.h"
#include "SDFileSystem.h"

DigitalOut myled1(LED1);

//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
InterruptIn LineSens1(PC_2);
InterruptIn LineSens2(PC_3);

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 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 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;

// 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");
    }
//Interrupt für Ziellinie
    LineSens1.rise(&setLine1);
    LineSens2.rise(&setLine2);


// 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_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");
                //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(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] = 2;
    map[X-1][Y] = 2;
    map[X][Y-1] = 2;
    map[X-1][Y-1] = 2;

// Karte auf SD speichern-------------------------------------------------------------------------------------

    FILE *fp = fopen("/sd/map.csv", "w");
    if (fp != NULL) {
        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");
        }
    } else {
        printf("Zugriff auf Karte verweigert");
    }
    fclose(fp);

    while(1) {
        myled1 = 1;
        wait(0.2);
        myled1 = 0;
        wait(0.2);
    }
}