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 "AutoDrive.h"
#include "RouteCalculation.h"
#include "SDFileSystem.h"
#include "FATFileSystem.h"

// Routenberechnung
// rückgabe 1d-Array route

    //SDFileSystem sdr(PB_5, PB_4, PB_3, PB_10, "sdr"); //mosi, miso, sclk, cs


/* Funktion berechnet neue Ausrichtung von Roboter.
    int turnDirection: Codierte Richtung in welche der Roboter drehen soll. (1 = Links, 2 = Rechts)
    int currentDirection: Codierte momentane Ausrichtung
    
    Return: Neue Momentanrichtung
*/
int directionControl(int turnDirection,int currentDirection){ // Links = 1, Rechts= 2
if (turnDirection == 1){ //Drehung nach Links
    currentDirection = currentDirection -1;
        if(currentDirection <= 0){
            printf("direction exception links\n");
        currentDirection = 4;
        }
    }
if (turnDirection == 2){ //Drehung nach Rechts
    currentDirection = currentDirection +1;
        if(currentDirection >= 5){
            printf("direction exception rechts\n");
        currentDirection = 1;
        }
    }
    return currentDirection;
}

/* Funktion zur Berechnung der Abfahrrute
    Berechnet aus einer 20x10 Matrix die schnellste route zum Zielpunkt.
    
    Return: Zeiger auf 2D-Array mit 2 Teilen und Anzahl Aktisonen Spalten. welcher von AUtoDrive() zum abfahren des gespeicherten Wegs benötigt wird.
*/
char *RouteCalculation(int (*map)[10]){

    printf("Routenberechnung wird gestartet...\n");
    //int map[20][10]; // Wird mit Werten des Mapping() gefüllt
    int X = 19;
    int Y = 9;
    int direction;
    int actionIndex; // Number des Befehls
    
    //char **route; 
    char *route = (char *)malloc(2*sizeof(char)); // Speicher muss alloziert werden!!
   

    // Pos in route[X,0]
    
    const char ZIEL = 5;
    const char FULLDRIVE = 1;
    const char TURNRIGHT = 2;
    const char TURNLEFT = 3;
    const char PLACETURN90 = 4;
    const char LEER = 0;

    int e; //Zähler
    
    // Pos in route[0,Y]
    
    
    // Codierung Richtungenänderungen
     const int DREHUNG_LINKS = 1;
     const int DREHUNG_RECHTS = 2;
    
    actionIndex = 0;
    int i = 0;
    printf("Variabeln inizialisiert\n");
    
    //-----------------------------------------------------------------------
    
    if (map[X-1][Y-1] == 0){
        route[actionIndex] = PLACETURN90;
        direction = 1;
    }else{
        route[actionIndex] = FULLDRIVE;
        direction = 4;
    }
    
    if(route[actionIndex] == FULLDRIVE){
        while(map[X-1][Y] == 0){
            if(X >= 1 | X <= 17){
                X = X - 2;
                i = i + 1;
                }else if(map[X-1][Y] == -1){X = X + 2;
                            printf("\n else statement");
                            map[X-1][Y] = 1;
            }
        } 
             
          
        route[actionIndex+1] = i;
        actionIndex = actionIndex +2;
    
    }
    printf("Aktion %d\n",actionIndex/2);
    printf("begin Schlaufe\n");
    
    while (map[X][Y] != 100){ // Ziel = Abbruchbedingung
        i = 0;
    //  -------------------------------------------------------------------------------------------  Grade Strecken fahren
     printf("Gerade wird kalkuliert...");
        switch (direction){
            case 1: while(map[X-1][Y-1] == 0){ // Gegen oben
                        if (Y >= 3 | Y <= 7){
                            Y = Y - 2;
                            if(map[X-1][Y-1] != -1){
                            i = i + 1;
                            }else{Y = Y + 2;
                            map[X-1][Y-1] = 1;
                            }
                        }
            } 
            
            printf("gegen oben ko X= %d, Y= %d",X,Y);  
            break;
            
            case 2: while(map[X][Y-1] == 0){ // Gegen rechts
                        if (X >= 3 | X <= 17){
                            X = X + 2;
                            if(map[X][Y-1] != -1){
                            i = i + 1;
                            }else{X = X - 2;
                            map[X][Y-1] = 1;
                            }
                        }
            }
            printf("gegen rechts ko X= %d, Y= %d",X,Y);  
            break;
            
            case 3: while(map[X][Y] == 0){ // Gegen unten
                        if (Y >= 3 | Y <= 7){
                            Y = Y + 2;
                            if(map[X][Y] != -1){
                            i = i + 1;
                            }else{Y = Y - 2;
                            map[X][Y] = 1;
                            }
                        }
            }
              printf("gegen unten ko X= %d, Y= %d",X,Y); 
            break;
            
            case 4: while(map[X-1][Y] == 0){ // Gegen links
            printf("\n case 4");
                        if (X >= 3 | X <= 17){
                            X = X - 2;
                            if(map[X-1][Y] != -1){
                            i = i + 1;
                            }else if(map[X-1][Y] == -1){X = X + 2;
                            printf("\n else statement");
                            map[X-1][Y] = 1;
                            }
                        }
            }
            printf("gegen links ko X= %d, Y= %d",X,Y); 
            break;  
        }
        
        actionIndex = actionIndex + 2; // Zur nächstesten Aktion
        printf("\nAktion %d\nRichtung %d",actionIndex/2, direction);
        route = (char *)realloc(route,(actionIndex+2)*sizeof(char)); // Speicher für neue Aktion initialisieren (+1 weil speicher mind. 1 mal char aber actionIndex start bei 0)
        route[actionIndex] = FULLDRIVE; // Fahrmodus auf geradeausfahren
        route[actionIndex+1] = i; // Länge der Strecke eintragen
        
    
    //  -------------------------------------------------------------------------------------------  Drehungen fahren
    printf("Drehung wird kalkuliert...");
    
        switch(direction){
            case 1: // Roboter gegen oben ausgerichtet
                    if(map[X][Y-1] == 0){ // gegen Rechts abbiegen
                    actionIndex = actionIndex + 2; // Zur nächstesten Aktion
                    char *route = (char*)realloc(route,(actionIndex+2)*sizeof(char)); // Speicher für neue Aktion initialisieren
                    route[actionIndex] = TURNRIGHT;
                    route[actionIndex+1] = LEER;
                    direction = directionControl(DREHUNG_RECHTS, direction);
                    }
                    else{
                    if(map[X-1][Y] == 0){ // gegen Links abbiegen
                    actionIndex = actionIndex + 2;
                    route = (char *)realloc(route,(actionIndex+2)*sizeof(char)); 
                    route[actionIndex]= TURNLEFT;
                    route[actionIndex+1] = LEER;
                    direction = directionControl(DREHUNG_LINKS, direction);
                    }
                    }
                    break;
                    
            case 2: // Roboter gegen rechts ausgerichtet
                    if(map[X-1][Y-1] == 0){ // gegen Oben abbiegen
                    actionIndex = actionIndex + 2;
                    route = (char *)realloc(route,(actionIndex+2)*sizeof(char)); 
                    route[actionIndex]= TURNLEFT;
                    route[actionIndex+1] = LEER;
                    direction = directionControl(DREHUNG_LINKS, direction);
                    }else{
                    if(map[X][Y] == 0){ // gegen Unten abbiegen
                    actionIndex = actionIndex + 2;
                    route = (char *)realloc(route,(actionIndex+2)*sizeof(char)); 
                    route[actionIndex] = TURNRIGHT;
                    route[actionIndex+1] = LEER;
                    direction = directionControl(DREHUNG_RECHTS, direction);
                        }   
                    }
                    
                    break;        
                    
            case 3: // Roboter gegen Unten ausgerichtet (Seitenverkehrt)
                    if(map[X][Y-1] == 0){ // gegen Rechts abbiegen
                    actionIndex = actionIndex + 2;
                    route = (char *)realloc(route,(actionIndex+2)*sizeof(char)); 
                    route[actionIndex] = TURNLEFT;
                    route[actionIndex+1] = LEER;
                    direction = directionControl(DREHUNG_LINKS, direction);
                    }else{
                    if(map[X-1][Y] == 0){ // gegen Links abbiegen 
                    actionIndex = actionIndex + 2;
                    route = (char *)realloc(route,(actionIndex+2)*sizeof(char)); 
                    route[actionIndex] = TURNRIGHT;
                    route[actionIndex+1] = LEER;
                    direction = directionControl(DREHUNG_RECHTS, direction);
                    }
                    }
                    break;
                    
            case 4: // Roboter gegen links ausgerichtet
                    if(map[X-1][Y-1] == 0){ // gegen oben abbiegen
                    actionIndex = actionIndex + 2;
                    route = (char *)realloc(route,(actionIndex+2)*sizeof(char)); 
                    route[actionIndex] = TURNRIGHT;
                    route[actionIndex+1] = LEER;
                    direction = directionControl(DREHUNG_RECHTS, direction);
                    }else{
                    if(map[X][Y] == 0){ // gegen unten abbiegen
                    actionIndex = actionIndex + 2;
                    route = (char *)realloc(route,(actionIndex+2)*sizeof(char)); 
                    route[actionIndex] = TURNLEFT;
                    route[actionIndex+1] = LEER;
                    direction = directionControl(DREHUNG_LINKS, direction);
                    } 
                    }
                    break;                      
            }
            printf(".Aktion %d\n",actionIndex/2);
            
            if (map[X][Y] == 100){
            actionIndex = actionIndex + 2;
            route = (char *)realloc(route,(actionIndex+2)*sizeof(char)); 
            route[actionIndex] = ZIEL;
            route[actionIndex+1] = ZIEL;
            printf("Ziel erreicht bei Aktion %d\n",actionIndex/2);
            }
            
            printf("Array Elemente: Action %d %d-%d, Action %d %d-%d \n",(actionIndex-2)/2,route[actionIndex-2],route[actionIndex-1],(actionIndex)/2,route[actionIndex],route[actionIndex+1]);
             
             
        } // Ende Kartografierungsschleife
    printf("Schlaufe beendet\n");
    
    for (e = 0; e <= actionIndex; e++){
        printf("%d,",route[e]);
        }
    
    return &route[actionIndex];
           
}