File content as of revision 11:68ee67d17320:

#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 -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 = 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] != -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 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]);
            }
            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();
    while(1) {
        myled1 = 1;
        wait(0.2);
        myled1 = 0;
        wait(0.2);
    }
}