RouteCalculation.cpp

00001 #include "mbed.h"
00002 #include "Controller.h"
00003 #include "MotorDriver.h"
00004 #include "ReadFinalLine.h"
00005 #include "ReadSensor.h"
00006 #include "Mapping.h"
00007 #include "AutoDrive.h"
00008 #include "RouteCalculation.h"
00009 #include "SDFileSystem.h"
00010 #include "FATFileSystem.h"
00011 
00012 // Routenberechnung
00013 // rückgabe 1d-Array route
00014 
00015     //SDFileSystem sdr(PB_5, PB_4, PB_3, PB_10, "sdr"); //mosi, miso, sclk, cs
00016 
00017 
00018 /* Funktion berechnet neue Ausrichtung von Roboter.
00019     int turnDirection: Codierte Richtung in welche der Roboter drehen soll. (1 = Links, 2 = Rechts)
00020     int currentDirection: Codierte momentane Ausrichtung
00021     
00022     Return: Neue Momentanrichtung
00023 */
00024 int directionControl(int turnDirection,int currentDirection){ // Links = 1, Rechts= 2
00025 if (turnDirection == 1){ //Drehung nach Links
00026     currentDirection = currentDirection -1;
00027         if(currentDirection <= 0){
00028             printf("direction exception links\n");
00029         currentDirection = 4;
00030         }
00031     }
00032 if (turnDirection == 2){ //Drehung nach Rechts
00033     currentDirection = currentDirection +1;
00034         if(currentDirection >= 5){
00035             printf("direction exception rechts\n");
00036         currentDirection = 1;
00037         }
00038     }
00039     return currentDirection;
00040 }
00041 
00042 /* Funktion zur Berechnung der Abfahrrute
00043     Berechnet aus einer 20x10 Matrix die schnellste route zum Zielpunkt.
00044     
00045     Return: Zeiger auf 2D-Array mit 2 Teilen und Anzahl Aktisonen Spalten. welcher von AUtoDrive() zum abfahren des gespeicherten Wegs benötigt wird.
00046 */
00047 char *RouteCalculation(int (*map)[10]){
00048 
00049     printf("Routenberechnung wird gestartet...\n");
00050     //int map[20][10]; // Wird mit Werten des Mapping() gefüllt
00051     int X = 19;
00052     int Y = 9;
00053     int direction;
00054     int actionIndex; // Number des Befehls
00055     
00056     //char **route; 
00057     char *route = (char *)malloc(2*sizeof(char)); // Speicher muss alloziert werden!!
00058    
00059 
00060     // Pos in route[X,0]
00061     
00062     const char ZIEL = 5;
00063     const char FULLDRIVE = 1;
00064     const char TURNRIGHT = 2;
00065     const char TURNLEFT = 3;
00066     const char PLACETURN90 = 4;
00067     const char LEER = 0;
00068 
00069     int e; //Zähler
00070     
00071     // Pos in route[0,Y]
00072     
00073     
00074     // Codierung Richtungenänderungen
00075      const int DREHUNG_LINKS = 1;
00076      const int DREHUNG_RECHTS = 2;
00077     
00078     actionIndex = 0;
00079     int i = 0;
00080     printf("Variabeln inizialisiert\n");
00081     
00082     //-----------------------------------------------------------------------
00083     
00084     if (map[X-1][Y-1] == 0){
00085         route[actionIndex] = PLACETURN90;
00086         direction = 1;
00087     }else{
00088         route[actionIndex] = FULLDRIVE;
00089         direction = 4;
00090     }
00091     
00092     if(route[actionIndex] == FULLDRIVE){
00093         while(map[X-1][Y] == 0){
00094             if(X >= 1 | X <= 17){
00095                 X = X - 2;
00096                 i = i + 1;
00097                 }else if(map[X-1][Y] == -1){X = X + 2;
00098                             printf("\n else statement");
00099                             map[X-1][Y] = 1;
00100             }
00101         } 
00102              
00103           
00104         route[actionIndex+1] = i;
00105         actionIndex = actionIndex +2;
00106     
00107     }
00108     printf("Aktion %d\n",actionIndex/2);
00109     printf("begin Schlaufe\n");
00110     
00111     while (map[X][Y] != 100){ // Ziel = Abbruchbedingung
00112         i = 0;
00113     //  -------------------------------------------------------------------------------------------  Grade Strecken fahren
00114      printf("Gerade wird kalkuliert...");
00115         switch (direction){
00116             case 1: while(map[X-1][Y-1] == 0){ // Gegen oben
00117                         if (Y >= 3 | Y <= 7){
00118                             Y = Y - 2;
00119                             if(map[X-1][Y-1] != -1){
00120                             i = i + 1;
00121                             }else{Y = Y + 2;
00122                             map[X-1][Y-1] = 1;
00123                             }
00124                         }
00125             } 
00126             
00127             printf("gegen oben ko X= %d, Y= %d",X,Y);  
00128             break;
00129             
00130             case 2: while(map[X][Y-1] == 0){ // Gegen rechts
00131                         if (X >= 3 | X <= 17){
00132                             X = X + 2;
00133                             if(map[X][Y-1] != -1){
00134                             i = i + 1;
00135                             }else{X = X - 2;
00136                             map[X][Y-1] = 1;
00137                             }
00138                         }
00139             }
00140             printf("gegen rechts ko X= %d, Y= %d",X,Y);  
00141             break;
00142             
00143             case 3: while(map[X][Y] == 0){ // Gegen unten
00144                         if (Y >= 3 | Y <= 7){
00145                             Y = Y + 2;
00146                             if(map[X][Y] != -1){
00147                             i = i + 1;
00148                             }else{Y = Y - 2;
00149                             map[X][Y] = 1;
00150                             }
00151                         }
00152             }
00153               printf("gegen unten ko X= %d, Y= %d",X,Y); 
00154             break;
00155             
00156             case 4: while(map[X-1][Y] == 0){ // Gegen links
00157             printf("\n case 4");
00158                         if (X >= 3 | X <= 17){
00159                             X = X - 2;
00160                             if(map[X-1][Y] != -1){
00161                             i = i + 1;
00162                             }else if(map[X-1][Y] == -1){X = X + 2;
00163                             printf("\n else statement");
00164                             map[X-1][Y] = 1;
00165                             }
00166                         }
00167             }
00168             printf("gegen links ko X= %d, Y= %d",X,Y); 
00169             break;  
00170         }
00171         
00172         actionIndex = actionIndex + 2; // Zur nächstesten Aktion
00173         printf("\nAktion %d\nRichtung %d",actionIndex/2, direction);
00174         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)
00175         route[actionIndex] = FULLDRIVE; // Fahrmodus auf geradeausfahren
00176         route[actionIndex+1] = i; // Länge der Strecke eintragen
00177         
00178     
00179     //  -------------------------------------------------------------------------------------------  Drehungen fahren
00180     printf("Drehung wird kalkuliert...");
00181     
00182         switch(direction){
00183             case 1: // Roboter gegen oben ausgerichtet
00184                     if(map[X][Y-1] == 0){ // gegen Rechts abbiegen
00185                     actionIndex = actionIndex + 2; // Zur nächstesten Aktion
00186                     char *route = (char*)realloc(route,(actionIndex+2)*sizeof(char)); // Speicher für neue Aktion initialisieren
00187                     route[actionIndex] = TURNRIGHT;
00188                     route[actionIndex+1] = LEER;
00189                     direction = directionControl(DREHUNG_RECHTS, direction);
00190                     }
00191                     else{
00192                     if(map[X-1][Y] == 0){ // gegen Links abbiegen
00193                     actionIndex = actionIndex + 2;
00194                     route = (char *)realloc(route,(actionIndex+2)*sizeof(char)); 
00195                     route[actionIndex]= TURNLEFT;
00196                     route[actionIndex+1] = LEER;
00197                     direction = directionControl(DREHUNG_LINKS, direction);
00198                     }
00199                     }
00200                     break;
00201                     
00202             case 2: // Roboter gegen rechts ausgerichtet
00203                     if(map[X-1][Y-1] == 0){ // gegen Oben abbiegen
00204                     actionIndex = actionIndex + 2;
00205                     route = (char *)realloc(route,(actionIndex+2)*sizeof(char)); 
00206                     route[actionIndex]= TURNLEFT;
00207                     route[actionIndex+1] = LEER;
00208                     direction = directionControl(DREHUNG_LINKS, direction);
00209                     }else{
00210                     if(map[X][Y] == 0){ // gegen Unten abbiegen
00211                     actionIndex = actionIndex + 2;
00212                     route = (char *)realloc(route,(actionIndex+2)*sizeof(char)); 
00213                     route[actionIndex] = TURNRIGHT;
00214                     route[actionIndex+1] = LEER;
00215                     direction = directionControl(DREHUNG_RECHTS, direction);
00216                         }   
00217                     }
00218                     
00219                     break;        
00220                     
00221             case 3: // Roboter gegen Unten ausgerichtet (Seitenverkehrt)
00222                     if(map[X][Y-1] == 0){ // gegen Rechts abbiegen
00223                     actionIndex = actionIndex + 2;
00224                     route = (char *)realloc(route,(actionIndex+2)*sizeof(char)); 
00225                     route[actionIndex] = TURNLEFT;
00226                     route[actionIndex+1] = LEER;
00227                     direction = directionControl(DREHUNG_LINKS, direction);
00228                     }else{
00229                     if(map[X-1][Y] == 0){ // gegen Links abbiegen 
00230                     actionIndex = actionIndex + 2;
00231                     route = (char *)realloc(route,(actionIndex+2)*sizeof(char)); 
00232                     route[actionIndex] = TURNRIGHT;
00233                     route[actionIndex+1] = LEER;
00234                     direction = directionControl(DREHUNG_RECHTS, direction);
00235                     }
00236                     }
00237                     break;
00238                     
00239             case 4: // Roboter gegen links ausgerichtet
00240                     if(map[X-1][Y-1] == 0){ // gegen oben abbiegen
00241                     actionIndex = actionIndex + 2;
00242                     route = (char *)realloc(route,(actionIndex+2)*sizeof(char)); 
00243                     route[actionIndex] = TURNRIGHT;
00244                     route[actionIndex+1] = LEER;
00245                     direction = directionControl(DREHUNG_RECHTS, direction);
00246                     }else{
00247                     if(map[X][Y] == 0){ // gegen unten abbiegen
00248                     actionIndex = actionIndex + 2;
00249                     route = (char *)realloc(route,(actionIndex+2)*sizeof(char)); 
00250                     route[actionIndex] = TURNLEFT;
00251                     route[actionIndex+1] = LEER;
00252                     direction = directionControl(DREHUNG_LINKS, direction);
00253                     } 
00254                     }
00255                     break;                      
00256             }
00257             printf(".Aktion %d\n",actionIndex/2);
00258             
00259             if (map[X][Y] == 100){
00260             actionIndex = actionIndex + 2;
00261             route = (char *)realloc(route,(actionIndex+2)*sizeof(char)); 
00262             route[actionIndex] = ZIEL;
00263             route[actionIndex+1] = ZIEL;
00264             printf("Ziel erreicht bei Aktion %d\n",actionIndex/2);
00265             }
00266             
00267             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]);
00268              
00269              
00270         } // Ende Kartografierungsschleife
00271     printf("Schlaufe beendet\n");
00272     
00273     for (e = 0; e <= actionIndex; e++){
00274         printf("%d,",route[e]);
00275         }
00276     
00277     return &route[actionIndex];
00278            
00279 }
00280     
00281     
00282     
00283     
00284     
00285