RainbowTeam / Mbed 2 deprecated ProjectTheseus

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

RouteCalculation.cpp@0:4a0b987c5c94, 2018-04-27 (annotated)

Committer:
Alexander_Zuest
Date:
Fri Apr 27 12:30:38 2018 +0000
Revision:
0:4a0b987c5c94
Child:
2:cb6bae534500
Base Version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Alexander_Zuest 0:4a0b987c5c94 1 // Routenberechnung
Alexander_Zuest 0:4a0b987c5c94 2 // rückgabe 2d-Array route
Alexander_Zuest 0:4a0b987c5c94 3
Alexander_Zuest 0:4a0b987c5c94 4 /* Funktion berechnet neue Ausrichtung von Roboter.
Alexander_Zuest 0:4a0b987c5c94 5 int turnDirection: Codierte Richtung in welche der Roboter drehen soll. (1 = Links, 2 = Rechts)
Alexander_Zuest 0:4a0b987c5c94 6 int currentDirection: Codierte momentane Ausrichtung
Alexander_Zuest 0:4a0b987c5c94 7
Alexander_Zuest 0:4a0b987c5c94 8 Return: Neue momentanrichtung
Alexander_Zuest 0:4a0b987c5c94 9 */
Alexander_Zuest 0:4a0b987c5c94 10 int directionControl(int turnDirection,int currentDirection){ // Links = 1, Rechts= 2
Alexander_Zuest 0:4a0b987c5c94 11 if (turnDirection = 1){ //Drehung nach Links
Alexander_Zuest 0:4a0b987c5c94 12 currentDirection = currentDirection -1;
Alexander_Zuest 0:4a0b987c5c94 13 if(currentDirection = 0){
Alexander_Zuest 0:4a0b987c5c94 14 currentDirection = 4;
Alexander_Zuest 0:4a0b987c5c94 15 }
Alexander_Zuest 0:4a0b987c5c94 16 }
Alexander_Zuest 0:4a0b987c5c94 17 if (turnDirection = 2){ //Drehung nach Rechts
Alexander_Zuest 0:4a0b987c5c94 18 currentDirection = currentDirection +1;
Alexander_Zuest 0:4a0b987c5c94 19 if(currentDirection = 5){
Alexander_Zuest 0:4a0b987c5c94 20 currentDirection = 1;
Alexander_Zuest 0:4a0b987c5c94 21 }
Alexander_Zuest 0:4a0b987c5c94 22 }
Alexander_Zuest 0:4a0b987c5c94 23 return currentDirection;
Alexander_Zuest 0:4a0b987c5c94 24 }
Alexander_Zuest 0:4a0b987c5c94 25
Alexander_Zuest 0:4a0b987c5c94 26 /* Funktion zur Berechnung der Abfahrrute
Alexander_Zuest 0:4a0b987c5c94 27 Berechnet aus einer 20x10 Matrix die schnellste route zum Zielpunkt.
Alexander_Zuest 0:4a0b987c5c94 28
Alexander_Zuest 0:4a0b987c5c94 29 Return: Zeiger auf 2D-Array mit 2 Teilen und Anzahl Aktionen Spalten. welcher von AUtoDrive() zum abfahren des gespeicherten Wegs benötigt wird.
Alexander_Zuest 0:4a0b987c5c94 30 */
Alexander_Zuest 0:4a0b987c5c94 31 int RouteCalculation(){
Alexander_Zuest 0:4a0b987c5c94 32
Alexander_Zuest 0:4a0b987c5c94 33 map[20,10]; // muss aus mapping importiert werden
Alexander_Zuest 0:4a0b987c5c94 34 int X = 19;
Alexander_Zuest 0:4a0b987c5c94 35 int Y = 9;
Alexander_Zuest 0:4a0b987c5c94 36 int direction;
Alexander_Zuest 0:4a0b987c5c94 37 int actionIndex; // Number des Befehls
Alexander_Zuest 0:4a0b987c5c94 38 char *route = (*char)malloc(2*sizeof(char)); // Speicher muss alloziert werden!!
Alexander_Zuest 0:4a0b987c5c94 39
Alexander_Zuest 0:4a0b987c5c94 40 // Pos in route[X,0]
Alexander_Zuest 0:4a0b987c5c94 41
Alexander_Zuest 0:4a0b987c5c94 42 const int ZIEL = 0
Alexander_Zuest 0:4a0b987c5c94 43 const int FULLDRIVE = 1
Alexander_Zuest 0:4a0b987c5c94 44 const int TURNRIGHT = 2
Alexander_Zuest 0:4a0b987c5c94 45 const int TURNLEFT = 3
Alexander_Zuest 0:4a0b987c5c94 46 const int PLACETURN90 = 4
Alexander_Zuest 0:4a0b987c5c94 47 const int LEER = 256
Alexander_Zuest 0:4a0b987c5c94 48
Alexander_Zuest 0:4a0b987c5c94 49
Alexander_Zuest 0:4a0b987c5c94 50 // Pos in route[0,Y]
Alexander_Zuest 0:4a0b987c5c94 51
Alexander_Zuest 0:4a0b987c5c94 52 const int TYPE = 0;
Alexander_Zuest 0:4a0b987c5c94 53 const int LENGHT = 1;
Alexander_Zuest 0:4a0b987c5c94 54
Alexander_Zuest 0:4a0b987c5c94 55 // Codierung Richtungenänderungen
Alexander_Zuest 0:4a0b987c5c94 56 const int DREHUNG_LINKS = 1
Alexander_Zuest 0:4a0b987c5c94 57 const int DREHUNG_RECHTS = 2
Alexander_Zuest 0:4a0b987c5c94 58
Alexander_Zuest 0:4a0b987c5c94 59 int actionIndex = 1;
Alexander_Zuest 0:4a0b987c5c94 60 int i = 0;
Alexander_Zuest 0:4a0b987c5c94 61
Alexander_Zuest 0:4a0b987c5c94 62 int counterS = 0;
Alexander_Zuest 0:4a0b987c5c94 63 int counterZ = 0;
Alexander_Zuest 0:4a0b987c5c94 64
Alexander_Zuest 0:4a0b987c5c94 65 //SD-Karte lesen
Alexander_Zuest 0:4a0b987c5c94 66 printf("Reading from SD card...");
Alexander_Zuest 0:4a0b987c5c94 67 fp = fopen("/sd/map.csv", "r");
Alexander_Zuest 0:4a0b987c5c94 68 if (fp != NULL) {
Alexander_Zuest 0:4a0b987c5c94 69 char *str = fgets(fp); // liesst gesammten Inhalt von map.csv ein
Alexander_Zuest 0:4a0b987c5c94 70 char *ptr = strtok(str, ",;");
Alexander_Zuest 0:4a0b987c5c94 71 for (counterZ; counterZ < 9; counterZ++) {
Alexander_Zuest 0:4a0b987c5c94 72
Alexander_Zuest 0:4a0b987c5c94 73 for (counterS; counterS < 19; counterS++){
Alexander_Zuest 0:4a0b987c5c94 74 map[counterS][counterZ]= atoi(ptr);
Alexander_Zuest 0:4a0b987c5c94 75 ptr = strtok(NULL, ",;"); // zum nächsten Element wechseln
Alexander_Zuest 0:4a0b987c5c94 76 }
Alexander_Zuest 0:4a0b987c5c94 77 }
Alexander_Zuest 0:4a0b987c5c94 78
Alexander_Zuest 0:4a0b987c5c94 79 fclose(fp);
Alexander_Zuest 0:4a0b987c5c94 80 }
Alexander_Zuest 0:4a0b987c5c94 81 else {
Alexander_Zuest 0:4a0b987c5c94 82 printf("Read-process failed!\nNo file available!\n\r");
Alexander_Zuest 0:4a0b987c5c94 83 }
Alexander_Zuest 0:4a0b987c5c94 84
Alexander_Zuest 0:4a0b987c5c94 85 //-----------------------------------------------------------------------
Alexander_Zuest 0:4a0b987c5c94 86
Alexander_Zuest 0:4a0b987c5c94 87 if (map[X-1, Y-1] == 0){
Alexander_Zuest 0:4a0b987c5c94 88 route[actionIndex,TYPE] = PLACETURN90;
Alexander_Zuest 0:4a0b987c5c94 89 direction = 1;
Alexander_Zuest 0:4a0b987c5c94 90 }else{
Alexander_Zuest 0:4a0b987c5c94 91 route[actionIndex,TYPE] = FULLDRIVE;
Alexander_Zuest 0:4a0b987c5c94 92 direction = 4;
Alexander_Zuest 0:4a0b987c5c94 93 }
Alexander_Zuest 0:4a0b987c5c94 94
Alexander_Zuest 0:4a0b987c5c94 95 if(route[actionIndex,TYPE] == FULLDRIVE){
Alexander_Zuest 0:4a0b987c5c94 96 while(map[X-1, Y] == 0){
Alexander_Zuest 0:4a0b987c5c94 97 if(X >= 1 | X <= 19){
Alexander_Zuest 0:4a0b987c5c94 98 X = X -2;
Alexander_Zuest 0:4a0b987c5c94 99 i = i + 1;
Alexander_Zuest 0:4a0b987c5c94 100 }
Alexander_Zuest 0:4a0b987c5c94 101 }
Alexander_Zuest 0:4a0b987c5c94 102 route[actionIndex,LENGHT] = i;
Alexander_Zuest 0:4a0b987c5c94 103 actionIndex = actionIndex +1;
Alexander_Zuest 0:4a0b987c5c94 104
Alexander_Zuest 0:4a0b987c5c94 105 }
Alexander_Zuest 0:4a0b987c5c94 106
Alexander_Zuest 0:4a0b987c5c94 107 while (map[X,Y] != 100){ // Ziel = Abbruchbedingung
Alexander_Zuest 0:4a0b987c5c94 108 i = 0;
Alexander_Zuest 0:4a0b987c5c94 109 // ------------------------------------------------------------------------------------------- Grade Strecken fahren
Alexander_Zuest 0:4a0b987c5c94 110 switch (direction){
Alexander_Zuest 0:4a0b987c5c94 111 case 1: while(map[X-1,Y-1 == 0]){ // Gegen oben
Alexander_Zuest 0:4a0b987c5c94 112 if (Y >= 1 | Y <= 7){
Alexander_Zuest 0:4a0b987c5c94 113 Y = Y - 2;
Alexander_Zuest 0:4a0b987c5c94 114 i = i + 1;
Alexander_Zuest 0:4a0b987c5c94 115 }
Alexander_Zuest 0:4a0b987c5c94 116 }
Alexander_Zuest 0:4a0b987c5c94 117 break;
Alexander_Zuest 0:4a0b987c5c94 118
Alexander_Zuest 0:4a0b987c5c94 119 case 2: while(map[X,Y-1 == 0]){ // Gegen rechts
Alexander_Zuest 0:4a0b987c5c94 120 if (Y >= 1 | Y <= 19){
Alexander_Zuest 0:4a0b987c5c94 121 X = X + 2;
Alexander_Zuest 0:4a0b987c5c94 122 i = i + 1;
Alexander_Zuest 0:4a0b987c5c94 123 }
Alexander_Zuest 0:4a0b987c5c94 124 }
Alexander_Zuest 0:4a0b987c5c94 125 break;
Alexander_Zuest 0:4a0b987c5c94 126
Alexander_Zuest 0:4a0b987c5c94 127 case 3: while(map[X,Y == 0]){ // Gegen unten
Alexander_Zuest 0:4a0b987c5c94 128 if (Y >= 1 | Y <= 7){
Alexander_Zuest 0:4a0b987c5c94 129 Y = Y + 2;
Alexander_Zuest 0:4a0b987c5c94 130 i = i + 1;
Alexander_Zuest 0:4a0b987c5c94 131 }
Alexander_Zuest 0:4a0b987c5c94 132 }
Alexander_Zuest 0:4a0b987c5c94 133 break;
Alexander_Zuest 0:4a0b987c5c94 134
Alexander_Zuest 0:4a0b987c5c94 135 case 4: while(map[X-1,Y == 0]){ // Gegen rechts
Alexander_Zuest 0:4a0b987c5c94 136 if (Y >= 1 | Y <= 19){
Alexander_Zuest 0:4a0b987c5c94 137 X = X - 2;
Alexander_Zuest 0:4a0b987c5c94 138 i = i + 1;
Alexander_Zuest 0:4a0b987c5c94 139 }
Alexander_Zuest 0:4a0b987c5c94 140 }
Alexander_Zuest 0:4a0b987c5c94 141 break;
Alexander_Zuest 0:4a0b987c5c94 142 }
Alexander_Zuest 0:4a0b987c5c94 143 actionIndex = actionIndex + 1; // Zur nächstesten Aktion
Alexander_Zuest 0:4a0b987c5c94 144 char *route = (*char)realloc(2*sizeof(char)); // Speicher für neue Aktion initialisieren
Alexander_Zuest 0:4a0b987c5c94 145 route[actionIndex,TYPE] = FULLDRIVE; // Fahrmodus auf geradeausfahren
Alexander_Zuest 0:4a0b987c5c94 146 route[actionIndex,LENGHT] = i; // Länge der Strecke eintragen
Alexander_Zuest 0:4a0b987c5c94 147
Alexander_Zuest 0:4a0b987c5c94 148
Alexander_Zuest 0:4a0b987c5c94 149 // ------------------------------------------------------------------------------------------- Drehungen fahren
Alexander_Zuest 0:4a0b987c5c94 150 switch(direction){
Alexander_Zuest 0:4a0b987c5c94 151 case 1: // Roboter gegen oben ausgerichtet
Alexander_Zuest 0:4a0b987c5c94 152 if(map[X,Y-1] == 0){ // gegen Rechts abbiegen
Alexander_Zuest 0:4a0b987c5c94 153 actionIndex = actionIndex + 1; // Zur nächstesten Aktion
Alexander_Zuest 0:4a0b987c5c94 154 char *route = (*char)realloc(2*sizeof(char)); // Speicher für neue Aktion initialisieren
Alexander_Zuest 0:4a0b987c5c94 155 route[actionIndex, TYPE] = TURNRIGHT;
Alexander_Zuest 0:4a0b987c5c94 156 route[actionIndex, LENGHT] = LEER;
Alexander_Zuest 0:4a0b987c5c94 157 direction = directionControl(DREHUNG_RECHTS, direction);
Alexander_Zuest 0:4a0b987c5c94 158 }
Alexander_Zuest 0:4a0b987c5c94 159 if(map[X-1,Y] == 0){ // gegen Links abbiegen
Alexander_Zuest 0:4a0b987c5c94 160 actionIndex = actionIndex + 1;
Alexander_Zuest 0:4a0b987c5c94 161 char *route = (*char)realloc(2*sizeof(char));
Alexander_Zuest 0:4a0b987c5c94 162 route[actionIndex, TYPE] = TURNLEFT;
Alexander_Zuest 0:4a0b987c5c94 163 route[actionIndex, LENGHT] = LEER;
Alexander_Zuest 0:4a0b987c5c94 164 direction = directionControl(DREHUNG_LINKS, direction);
Alexander_Zuest 0:4a0b987c5c94 165 }
Alexander_Zuest 0:4a0b987c5c94 166 case 2: // Roboter gegen rechts ausgerichtet
Alexander_Zuest 0:4a0b987c5c94 167 if(map[X-1,Y-1] == 0){ // gegen Oben abbiegen
Alexander_Zuest 0:4a0b987c5c94 168 actionIndex = actionIndex + 1;
Alexander_Zuest 0:4a0b987c5c94 169 char *route = (*char)realloc(2*sizeof(char));
Alexander_Zuest 0:4a0b987c5c94 170 route[actionIndex, TYPE] = TURNLEFT;
Alexander_Zuest 0:4a0b987c5c94 171 route[actionIndex, LENGHT] = LEER;
Alexander_Zuest 0:4a0b987c5c94 172 direction = directionControl(DREHUNG_LINKS, direction);
Alexander_Zuest 0:4a0b987c5c94 173 }
Alexander_Zuest 0:4a0b987c5c94 174 if(map[X,Y] == 0){ // gegen Unten abbiegen
Alexander_Zuest 0:4a0b987c5c94 175 actionIndex = actionIndex + 1;
Alexander_Zuest 0:4a0b987c5c94 176 char *route = (*char)realloc(2*sizeof(char));
Alexander_Zuest 0:4a0b987c5c94 177 route[actionIndex, TYPE] = TURNRIGHT;
Alexander_Zuest 0:4a0b987c5c94 178 route[actionIndex, LENGHT] = LEER;
Alexander_Zuest 0:4a0b987c5c94 179 direction = directionControl(DREHUNG_RECHTS, direction);
Alexander_Zuest 0:4a0b987c5c94 180 }
Alexander_Zuest 0:4a0b987c5c94 181 case 3: // Roboter gegen Unten ausgerichtet (Seitenverkehrt)
Alexander_Zuest 0:4a0b987c5c94 182 if(map[X,Y-1] == 0){ // gegen Rechts abbiegen
Alexander_Zuest 0:4a0b987c5c94 183 actionIndex = actionIndex + 1;
Alexander_Zuest 0:4a0b987c5c94 184 char *route = (*char)realloc(2*sizeof(char));
Alexander_Zuest 0:4a0b987c5c94 185 route[actionIndex, TYPE] = TURNLEFT;
Alexander_Zuest 0:4a0b987c5c94 186 route[actionIndex, LENGHT] = LEER;
Alexander_Zuest 0:4a0b987c5c94 187 direction = directionControl(DREHUNG_LINKS, direction);
Alexander_Zuest 0:4a0b987c5c94 188 }
Alexander_Zuest 0:4a0b987c5c94 189 if(map[X-1,Y] == 0){ // gegen Links abbiegen
Alexander_Zuest 0:4a0b987c5c94 190 actionIndex = actionIndex + 1;
Alexander_Zuest 0:4a0b987c5c94 191 char *route = (*char)realloc(2*sizeof(char));
Alexander_Zuest 0:4a0b987c5c94 192 route[actionIndex, TYPE] = TURNRIGHT;
Alexander_Zuest 0:4a0b987c5c94 193 route[actionIndex, LENGHT] = LEER;
Alexander_Zuest 0:4a0b987c5c94 194 direction = directionControl(DREHUNG_RECHTS, direction);
Alexander_Zuest 0:4a0b987c5c94 195 }
Alexander_Zuest 0:4a0b987c5c94 196 case 4: // Roboter gegen links ausgerichtet
Alexander_Zuest 0:4a0b987c5c94 197 if(map[X-1,Y-1] == 0){ // gegen oben abbiegen
Alexander_Zuest 0:4a0b987c5c94 198 actionIndex = actionIndex + 1;
Alexander_Zuest 0:4a0b987c5c94 199 char *route = (*char)realloc(2*sizeof(char));
Alexander_Zuest 0:4a0b987c5c94 200 route[actionIndex, TYPE] = TURNRIGHT;
Alexander_Zuest 0:4a0b987c5c94 201 route[actionIndex, LENGHT] = LEER;
Alexander_Zuest 0:4a0b987c5c94 202 direction = directionControl(DREHUNG_RECHTS, direction);
Alexander_Zuest 0:4a0b987c5c94 203 }
Alexander_Zuest 0:4a0b987c5c94 204 if(map[X,Y] == 0){ // gegen unten abbiegen
Alexander_Zuest 0:4a0b987c5c94 205 actionIndex = actionIndex + 1;
Alexander_Zuest 0:4a0b987c5c94 206 char *route = (*char)realloc(2*sizeof(char));
Alexander_Zuest 0:4a0b987c5c94 207 route[actionIndex, TYPE] = TURNLEFT;
Alexander_Zuest 0:4a0b987c5c94 208
Alexander_Zuest 0:4a0b987c5c94 209 direction = directionControl(DREHUNG_LINKS, direction);
Alexander_Zuest 0:4a0b987c5c94 210 }
Alexander_Zuest 0:4a0b987c5c94 211 }
Alexander_Zuest 0:4a0b987c5c94 212 if (map[X,Y] == 100){
Alexander_Zuest 0:4a0b987c5c94 213 char *route = (*char)realloc(2*sizeof(char));
Alexander_Zuest 0:4a0b987c5c94 214 actionIndex = actionIndex + 1;
Alexander_Zuest 0:4a0b987c5c94 215 route[actionIndex, TYPE] = ZIEL;
Alexander_Zuest 0:4a0b987c5c94 216 route[actionIndex, LENGHT] = ZIEL;
Alexander_Zuest 0:4a0b987c5c94 217 }
Alexander_Zuest 0:4a0b987c5c94 218
Alexander_Zuest 0:4a0b987c5c94 219
Alexander_Zuest 0:4a0b987c5c94 220 } // Ende Kartografierungsschleife
Alexander_Zuest 0:4a0b987c5c94 221
Alexander_Zuest 0:4a0b987c5c94 222 return *route;
Alexander_Zuest 0:4a0b987c5c94 223
Alexander_Zuest 0:4a0b987c5c94 224 }
Alexander_Zuest 0:4a0b987c5c94 225
Alexander_Zuest 0:4a0b987c5c94 226
Alexander_Zuest 0:4a0b987c5c94 227
Alexander_Zuest 0:4a0b987c5c94 228
Alexander_Zuest 0:4a0b987c5c94 229
Alexander_Zuest 0:4a0b987c5c94 230