File content as of revision 5:695c5531f65e:

#include "mbed.h"
#include "Controller.h"
#include "MotorDriver.h"
#include "ReadFinalLine.h"
#include "ReadSensor.h"
#include "Mapping.h"
#include "AutoDrive.h"
#include "RouteCalculation.h"

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

/* 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){
        currentDirection = 4;
        }
    }
if (turnDirection == 2){ //Drehung nach Rechts
    currentDirection = currentDirection +1;
        if(currentDirection == 5){
        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 Aktionen Spalten. welcher von AUtoDrive() zum abfahren des gespeicherten Wegs benötigt wird.
*/
int RouteCalculation(){
    
    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(sizeof(char)); // Speicher muss alloziert werden!!
   
    
    // Pos in route[X,0]
    
    const char ZIEL = 0;
    const char FULLDRIVE = 1;
    const char TURNRIGHT = 2;
    const char TURNLEFT = 3;
    const char PLACETURN90 = 4;
    const char LEER = 256;
    
    
    // Pos in route[0,Y]
    
    const int TYPE = 0;
    const int LENGHT = 1;
    
    // Codierung Richtungenänderungen
     const int DREHUNG_LINKS = 1;
     const int DREHUNG_RECHTS = 2;
    
    actionIndex = 1;
    int i = 0;
    
    int counterS = 0;
    int counterZ = 0;
    
    char *str;
     
    //SD-Karte lesen
    printf("Reading from SD card...");
    FILE *fp = fopen("/sd/map.csv", "r");
    if (fp != NULL) {
       fgets(str,500,fp);       // liesst gesammten Inhalt von map.csv ein
       char *ptr = strtok(str, ",;");
        for (counterZ; counterZ < 9; counterZ++) {
            
            for (counterS; counterS < 19; counterS++){
                map[counterS][counterZ] = atoi(ptr);
                ptr = strtok(NULL, ",;"); // zum  nächsten Element wechseln
                }
            }
               
    fclose(fp);   
    }
    else {
        printf("Read-process failed!\nNo file available!\n\r");
    }
    
    //-----------------------------------------------------------------------
    
    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 <= 19){
                X = X -2;
                i = i + 1;
            }
        }   
        route[actionIndex+1] = i;
        actionIndex = actionIndex +1;
        
    }
    
    while (map[X][Y] != 100){ // Ziel = Abbruchbedingung
        i = 0;
    //  -------------------------------------------------------------------------------------------  Grade Strecken fahren
        switch (direction){  
            case 1: while(map[X-1][Y-1] == 0){ // Gegen oben
                        if (Y >= 1 | Y <= 7){
                            Y = Y - 2;
                            i = i + 1;
                        }
            }   
            break;
            
            case 2: while(map[X][Y-1] == 0){ // Gegen rechts
                        if (Y >= 1 | Y <= 19){
                            X = X + 2;
                            i = i + 1;
                        }
            }
            break;
            
            case 3: while(map[X][Y] == 0){ // Gegen unten
                        if (Y >= 1 | Y <= 7){
                            Y = Y + 2;
                            i = i + 1;
                        }
            }
            break;
            
            case 4: while(map[X-1][Y] == 0){ // Gegen rechts
                        if (Y >= 1 | Y <= 19){
                            X = X - 2;
                            i = i + 1;
                        }
            }
            break;  
        }
        actionIndex = actionIndex + 2; // Zur nächstesten Aktion
        route = (char *)realloc(route,(actionIndex+2)*sizeof(char)); // Speicher für neue Aktion initialisieren
        route[actionIndex] = FULLDRIVE; // Fahrmodus auf geradeausfahren
        route[actionIndex+1] = i; // Länge der Strecke eintragen
        
    
    //  -------------------------------------------------------------------------------------------  Drehungen fahren
        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);
                    }
                    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);
                    }
            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);
                    }
                    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);
                    }           
            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);
                    }
                    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);
                    }
            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);
                    }
                    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);
                    }                       
            }
            if (map[X][Y] == 100){
            route = (char *)realloc(route,(actionIndex+2)*sizeof(char)); 
            actionIndex = actionIndex + 2;
            route[actionIndex] = ZIEL;
            route[actionIndex+1] = ZIEL;
            }
            
            
        } // Ende Kartografierungsschleife
        
    return *route;
            
    }