File content as of revision 14:0caa7b93af7a:

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

DigitalOut myled1(LED1);

// 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(int (*map)[10]){ //------------------------------------------------------------------------------------------------------ Mapping


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;
int filler = -1;
for (f = 0; f < 20; f++) {
    for (g = 0; g < 10; g++) {
        map[f][g] = filler;
        }
    }

int speed = 2;

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;

}
else{
    currentDirection = turnRight(currentDirection);
    driveOne(1,speed);
    Y = Y-2;

}
//Interrupt für Ziellinie
LineSens1.rise(&setLine1);
LineSens2.rise(&setLine2);  


// Farhralgorithmus

while (drive == 1){ 


    // Variabeln zurücksetzen
     P_straight = 1;
     P_rightTurn = 1;
     P_leftTurn = 1;


 // 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){
// --------------------------------------------------------------------------- 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;
        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;
        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;
        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][Y-1] = 1;

        break;
    default: printf("Error: Direction out of definition\n");
    }
  }      
    //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);
        driveOne(1,speed);
        } else{
            if (P_straight == 0){
                driveOne(1,speed); 
            } else{
                if(P_leftTurn == 0){
                    currentDirection = turnLeft(currentDirection);
                    driveOne(1,speed);
                    } else{
                        currentDirection = turnRight(currentDirection);
                        currentDirection = turnRight(currentDirection);
                        driveOne(1,speed);
                        }
            }
        }
        
        
        
    // 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
    }
    
    //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

// Zielmanoever ausführen-------------------------------------------------------------------------------------

map[X][Y] = 100;
map[X-1][Y] = 100;
map[X][Y-1] = 100;
map[X-1][Y-1] = 100;
 
 int k = 0;
 int i = 0;
 
        for (int i = 0; i <= 9; i++) {
            for (int k = 0; k <= 19; k++){
                printf("%d;",map[k][i]); 
            }
            printf("\n");
            }
 }