Revision 14:0caa7b93af7a, committed 2018-05-23

Comitter:

Alexander_Zuest

Date:

Wed May 23 11:50:58 2018 +0000

Parent:

9:5802ccfcc984

Child:

15:31d09ee65cf1

Commit message:

50% Chance for working Routecalculation

Changed in this revision

--- a/AutoDrive.cpp	Tue May 08 12:32:31 2018 +0000
+++ b/AutoDrive.cpp	Wed May 23 11:50:58 2018 +0000
@@ -2,16 +2,16 @@
 #include "MotorDriver.h"
 #include "ReadFinalLine.h"
 #include "ReadSensor.h"
-#include "Mapping.h"
 #include "AutoDrive.h"
 #include "RouteCalculation.h" 
-    
+
+    void autoDrive(char route[]){
+        
     int action = 0;
     int speed = 3;
     int placeholder = 1;
-    void AutoDrive(const int route[]){
-        
-    while(route[action] != 100){
+    
+    while(route[action] != 5){
         
         switch(route[action]){
             case 1: // Fulldrive
@@ -33,7 +33,7 @@
             default: printf("Error: route could not be red!");
         }
         
-    action = action + 1;    
+    action = action + 2;    
     }
     printf("The target destination has been sucessfully reached!");
     

--- a/AutoDrive.h	Tue May 08 12:32:31 2018 +0000
+++ b/AutoDrive.h	Wed May 23 11:50:58 2018 +0000
@@ -1,8 +1,8 @@
-#ifndef Mapping_H
-#define Mapping_H
+#ifndef Auto_H
+#define Auto_H
 
-//Funktionsdeklaration
-void AutoDrive(void);
- 
+// Funktionsdeklaration
+void autoDrive(char route[]);
+
 #endif
  

--- a/Mapping.cpp	Tue May 08 12:32:31 2018 +0000
+++ b/Mapping.cpp	Wed May 23 11:50:58 2018 +0000
@@ -4,13 +4,11 @@
 #include "ReadFinalLine.h"
 #include "ReadSensor.h"
 #include "Mapping.h"
-#include "SDFileSystem.h"
+//#include "SDFileSystem.h"
+//#include "FATFileSystem.h"
 
 DigitalOut myled1(LED1);
 
-//debug
-//DigitalOut myled(LED2);
-
 // Funktionsinitalisierungen
 int readSensor(int SensorNummer);// Liesst Ultraschallsensoren 1-3 aus
 
@@ -22,23 +20,27 @@
 
 //  int wait(int time); Systemfunktion für Pausen
 
-SDFileSystem sd(PB_5, PB_4, PB_3, PB_10, "sd"); //mosi, miso, sclk, cs
+//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;
+void mapping(int (*map)[10]){ //------------------------------------------------------------------------------------------------------ Mapping
 
 
-int map[20][10]; // 20 Zeilen, 10 Spalten, Karte gefüllt mit -1 für keine Daten
+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;
-for (f = 0; f < 3; f++) {
-for (g = 0; g < 5; g++) {
-map[f][g] = -1;
-}
-}
+int filler = -1;
+for (f = 0; f < 20; f++) {
+    for (g = 0; g < 10; g++) {
+        map[f][g] = filler;
+        }
+    }
 
 int speed = 2;
 
@@ -61,7 +63,7 @@
 int currentDirection = startrichtung;
 
 // Verzögerung zwischen Aktionen in ms
-int pause = 2;
+//int pause = 2;
 
 // Zustandsvariabel während Fahrt = 1 bei Ziel =0
 int drive = 1;
@@ -71,17 +73,17 @@
 if (readSensor(front) == false){    // Gegen vorne freie Fahrt (kein Hindernis -> 0)
     map[X-1][Y] = 0;
     }else{
-        map [X-1][Y] = 1;
+        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-1][Y-1] = 1;
 }
 
-map[X][ Y-1] = 1; // ergibt sich aus Feldaufbau und Startrichtung
-map[X ][ Y] = 1;
+map[X][Y-1] = 1; // ergibt sich aus Feldaufbau und Startrichtung
+map[X][Y] = 1;
 
 
 // Erster Schritt
@@ -89,32 +91,120 @@
 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);  
+while (drive == 1){ 
+
 
     // Variabeln zurücksetzen
      P_straight = 1;
      P_rightTurn = 1;
      P_leftTurn = 1;
 
-    int s = 12;
+
+ // 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){
@@ -134,7 +224,7 @@
         
         // Kartografierung Feld rechts
         if (readSensor(right) == false){
-            map[X][Y-1] = 0;
+           map[X][Y-1] = 0;
             P_rightTurn = 0;
         }
         else{
@@ -153,8 +243,6 @@
         
         // Kartografierung Feld nach hinten
         map[X][Y] = 1;
-            // debug
-    //printf("case 1 \n");
         break;
 
 // --------------------------------------------------------------------------- 2 Ausrichtung Rechts
@@ -190,8 +278,6 @@
         
         // Kartografierung Feld nach hinten
         map[X-1][Y] = 1;
-            // debug
-    //printf("case 2 \n");
         break;
 // --------------------------------------------------------------------------- 3 Ausrichtung Unten
         case 3:
@@ -226,19 +312,17 @@
         
         // 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;
+           map[X-1][Y] = 0;
             P_straight = 0;
         }
         else{
-            map[X-1][Y] = 1;
+           map[X-1][Y] = 1;
         }
         
         // Kartografierung Feld rechts
@@ -261,23 +345,12 @@
         }
         
         // Kartografierung Feld nach hinten
-        map[X-1][Y-1] = 1;
-         // debug
-    //printf("case 4\n");
+        map[X][Y-1] = 1;
 
         break;
-    default: //printf("Error: Direction out of definition\n");
+    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 );
@@ -290,35 +363,24 @@
 
     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){
@@ -344,29 +406,7 @@
         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;
-        }
-        
-    }
+    //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
 
@@ -376,49 +416,15 @@
 map[X-1][Y] = 100;
 map[X][Y-1] = 100;
 map[X-1][Y-1] = 100;
-
-// Karte auf SD speichern-------------------------------------------------------------------------------------
-
+ 
+ int k = 0;
+ int i = 0;
  
-//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]);
+        for (int i = 0; i <= 9; i++) {
+            for (int k = 0; k <= 19; k++){
+                printf("%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");
-    }
+            printf("\n");
+            }
+ }
 
-    //Unmount the filesystem
-    sd.unmount();
-    while(1) {
-        myled1 = 1;
-        wait(0.2);
-        myled1 = 0;
-        wait(0.2);
-    }
-}
-
-
-
-
-
-
-
-
-
-
-
-

--- a/Mapping.h	Tue May 08 12:32:31 2018 +0000
+++ b/Mapping.h	Wed May 23 11:50:58 2018 +0000
@@ -2,7 +2,8 @@
 #define Mapping_H
 
 //Funktionsdeklaration
-void mapping(void);
+void mapping(int (*map)[10]);
+
  
 #endif
  

--- a/RouteCalculation.cpp	Tue May 08 12:32:31 2018 +0000
+++ b/RouteCalculation.cpp	Wed May 23 11:50:58 2018 +0000
@@ -7,26 +7,32 @@
 #include "AutoDrive.h"
 #include "RouteCalculation.h"
 #include "SDFileSystem.h"
+#include "FATFileSystem.h"
 
 // Routenberechnung
-// rückgabe 2d-Array route
+// 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
+    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){
+        if(currentDirection <= 0){
+            printf("direction exception links\n");
         currentDirection = 4;
         }
     }
 if (turnDirection == 2){ //Drehung nach Rechts
     currentDirection = currentDirection +1;
-        if(currentDirection == 5){
+        if(currentDirection >= 5){
+            printf("direction exception rechts\n");
         currentDirection = 1;
         }
     }
@@ -36,73 +42,46 @@
 /* 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.
+    Return: Zeiger auf 2D-Array mit 2 Teilen und Anzahl Aktisonen 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; 
+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(sizeof(char)); // Speicher muss alloziert werden!!
+    char *route = (char *)malloc(2*sizeof(char)); // Speicher muss alloziert werden!!
    
-   SDFileSystem sd(PB_5, PB_4, PB_3, PB_10, "sd"); //mosi, miso, sclk, cs
-    
+
     // Pos in route[X,0]
     
-    const char ZIEL = 0;
+    const char ZIEL = 5;
     const char FULLDRIVE = 1;
     const char TURNRIGHT = 2;
     const char TURNLEFT = 3;
     const char PLACETURN90 = 4;
-    const char LEER = 256;
-    
+    const char LEER = 0;
+
+    int e; //Zähler
     
     // 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;
+    actionIndex = 0;
     int i = 0;
-    
-    int counterS = 0;
-    int counterZ = 0;
-    
-    char *str;
-     
-    //SD-Karte lesen
-    sd.mount();
-    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");
-    }
-    sd.unmount();
+    printf("Variabeln inizialisiert\n");
     
     //-----------------------------------------------------------------------
     
-    if (map[X-1][ Y-1] == 0){
+    if (map[X-1][Y-1] == 0){
         route[actionIndex] = PLACETURN90;
         direction = 1;
     }else{
@@ -112,59 +91,94 @@
     
     if(route[actionIndex] == FULLDRIVE){
         while(map[X-1][Y] == 0){
-            if(X >= 1 | X <= 19){
-                X = X -2;
+            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 +1;
-        
+        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
-        switch (direction){  
+     printf("Gerade wird kalkuliert...");
+        switch (direction){
             case 1: while(map[X-1][Y-1] == 0){ // Gegen oben
-                        if (Y >= 1 | Y <= 7){
+                        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 (Y >= 1 | Y <= 19){
+                        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 >= 1 | Y <= 7){
+                        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 rechts
-                        if (Y >= 1 | Y <= 19){
+            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
-        route = (char *)realloc(route,(actionIndex+2)*sizeof(char)); // Speicher für neue Aktion initialisieren
+        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
@@ -174,6 +188,7 @@
                     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)); 
@@ -181,6 +196,9 @@
                     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;
@@ -188,14 +206,18 @@
                     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;
@@ -203,7 +225,7 @@
                     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)); 
@@ -211,6 +233,9 @@
                     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;
@@ -218,28 +243,40 @@
                     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)); 
-            actionIndex = actionIndex + 2;
             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
-        
-    return *route;
-            
-    }
+    printf("Schlaufe beendet\n");
+    
+    for (e = 0; e <= actionIndex; e++){
+        printf("%d,",route[e]);
+        }
+    
+    return &route[actionIndex];
+           
+}
     
     
     

--- a/RouteCalculation.h	Tue May 08 12:32:31 2018 +0000
+++ b/RouteCalculation.h	Wed May 23 11:50:58 2018 +0000
@@ -1,8 +1,10 @@
-#ifndef Mapping_H
-#define Mapping_H
+#ifndef RouteCalculation_H
+#define RouteCalculation_H
+
 
 //Funktionsdeklaration
-void RouteCalculation(void);
+char *RouteCalculation(int (*map)[10]);
+
  
 #endif
  
\ No newline at end of file

--- a/SDFileSystem.h	Tue May 08 12:32:31 2018 +0000
+++ b/SDFileSystem.h	Wed May 23 11:50:58 2018 +0000
@@ -40,7 +40,7 @@
  *     printf("\nWriting to SD card...");
  *     FILE *fp = fopen("/sd/sdtest.txt", "w");
  *     if (fp != NULL) {
- *         fprintf(fp, "We're writing to an SD card!");
+ *         fprintf(fp, "We're writing to an SD card! yay");
  *         fclose(fp);
  *         printf("success!\n");
  *     } else {

--- a/main.cpp	Tue May 08 12:32:31 2018 +0000
+++ b/main.cpp	Wed May 23 11:50:58 2018 +0000
@@ -6,28 +6,33 @@
 #include "Mapping.h"
 #include "AutoDrive.h"
 #include "RouteCalculation.h"
-#include "SDFileSystem.h"
-#include "FATFileSystem.h"
-#include "ff.h"
+
+
+char* route;
+
+// inizialisieren der Karte
+int map[20][10];
+
 
 
 
-//char route[];
-
-int main()
-{
+int main(){
+    printf("startup");
     startup();
+    wait(5);
     
     // Mapping Modus
-    mapping();
+    printf("mapping");
+    mapping(map);
     
-    // Schalter einbinden!
     
     // Autodrive Modus
-    /*
-    startup();
-    Route = RouteCalculation();
-    AutoDrive(Route);
-    */
+    printf("Route wird berechnet");
+    route = RouteCalculation(map);
+    printf("Berechnung Abgeschlossen");
+    //wait(20);
+    //printf("strecke wird abgefahren");
+    //autoDrive(route);
+    
 }