Diff: RouteCalculation.cpp

Revision:

0:4a0b987c5c94

Child:

2:cb6bae534500

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RouteCalculation.cpp	Fri Apr 27 12:30:38 2018 +0000
@@ -0,0 +1,230 @@
+// 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(){
+    
+    map[20,10]; // muss aus mapping importiert werden
+    int X = 19; 
+    int Y = 9;
+    int direction;
+    int actionIndex; // Number des Befehls
+    char *route = (*char)malloc(2*sizeof(char)); // Speicher muss alloziert werden!!
+    
+    // Pos in route[X,0]
+    
+    const int ZIEL = 0
+    const int FULLDRIVE = 1
+    const int TURNRIGHT = 2
+    const int TURNLEFT = 3
+    const int PLACETURN90 = 4
+    const int 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
+    
+    int actionIndex = 1;
+    int i = 0;
+    
+    int counterS = 0;
+    int counterZ = 0;
+
+    //SD-Karte lesen
+    printf("Reading from SD card...");
+    fp = fopen("/sd/map.csv", "r");
+    if (fp != NULL) {
+       char *str = fgets(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,TYPE] = PLACETURN90;
+        direction = 1;
+    }else{
+        route[actionIndex,TYPE] = FULLDRIVE;
+        direction = 4;
+    }
+    
+    if(route[actionIndex,TYPE] == FULLDRIVE){
+        while(map[X-1, Y] == 0){
+            if(X >= 1 | X <= 19){
+                X = X -2;
+                i = i + 1;
+            }
+        }   
+        route[actionIndex,LENGHT] = 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 + 1; // Zur nächstesten Aktion
+        char *route = (*char)realloc(2*sizeof(char)); // Speicher für neue Aktion initialisieren
+        route[actionIndex,TYPE] = FULLDRIVE; // Fahrmodus auf geradeausfahren
+        route[actionIndex,LENGHT] = 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 + 1; // Zur nächstesten Aktion
+                    char *route = (*char)realloc(2*sizeof(char)); // Speicher für neue Aktion initialisieren
+                    route[actionIndex, TYPE] = TURNRIGHT;
+                    route[actionIndex, LENGHT] = LEER;
+                    direction = directionControl(DREHUNG_RECHTS, direction);
+                    }
+                    if(map[X-1,Y] == 0){ // gegen Links abbiegen
+                    actionIndex = actionIndex + 1;
+                    char *route = (*char)realloc(2*sizeof(char)); 
+                    route[actionIndex, TYPE] = TURNLEFT;
+                    route[actionIndex, LENGHT] = LEER;
+                    direction = directionControl(DREHUNG_LINKS, direction);
+                    }
+            case 2: // Roboter gegen rechts ausgerichtet
+                    if(map[X-1,Y-1] == 0){ // gegen Oben abbiegen
+                    actionIndex = actionIndex + 1;
+                    char *route = (*char)realloc(2*sizeof(char)); 
+                    route[actionIndex, TYPE] = TURNLEFT;
+                    route[actionIndex, LENGHT] = LEER;
+                    direction = directionControl(DREHUNG_LINKS, direction);
+                    }
+                    if(map[X,Y] == 0){ // gegen Unten abbiegen
+                    actionIndex = actionIndex + 1;
+                    char *route = (*char)realloc(2*sizeof(char)); 
+                    route[actionIndex, TYPE] = TURNRIGHT;
+                    route[actionIndex, LENGHT] = LEER;
+                    direction = directionControl(DREHUNG_RECHTS, direction);
+                    }           
+            case 3: // Roboter gegen Unten ausgerichtet (Seitenverkehrt)
+                    if(map[X,Y-1] == 0){ // gegen Rechts abbiegen
+                    actionIndex = actionIndex + 1;
+                    char *route = (*char)realloc(2*sizeof(char)); 
+                    route[actionIndex, TYPE] = TURNLEFT;
+                    route[actionIndex, LENGHT] = LEER;
+                    direction = directionControl(DREHUNG_LINKS, direction);
+                    }
+                    if(map[X-1,Y] == 0){ // gegen Links abbiegen 
+                    actionIndex = actionIndex + 1;
+                    char *route = (*char)realloc(2*sizeof(char)); 
+                    route[actionIndex, TYPE] = TURNRIGHT;
+                    route[actionIndex, LENGHT] = LEER;
+                    direction = directionControl(DREHUNG_RECHTS, direction);
+                    }
+            case 4: // Roboter gegen links ausgerichtet
+                    if(map[X-1,Y-1] == 0){ // gegen oben abbiegen
+                    actionIndex = actionIndex + 1;
+                    char *route = (*char)realloc(2*sizeof(char)); 
+                    route[actionIndex, TYPE] = TURNRIGHT;
+                    route[actionIndex, LENGHT] = LEER;
+                    direction = directionControl(DREHUNG_RECHTS, direction);
+                    }
+                    if(map[X,Y] == 0){ // gegen unten abbiegen
+                    actionIndex = actionIndex + 1;
+                    char *route = (*char)realloc(2*sizeof(char)); 
+                    route[actionIndex, TYPE] = TURNLEFT;
+                    
+                    direction = directionControl(DREHUNG_LINKS, direction);
+                    }                       
+            }
+            if (map[X,Y] == 100){
+            char *route = (*char)realloc(2*sizeof(char)); 
+            actionIndex = actionIndex + 1;
+            route[actionIndex, TYPE] = ZIEL;
+            route[actionIndex, LENGHT] = ZIEL;
+            }
+            
+            
+        } // Ende Kartografierungsschleife
+        
+    return *route;
+            
+    }
+    
+    
+    
+    
+    
+