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@4:aff0722b4e50, 2018-04-28 (annotated)

Committer:
Alexander_Zuest
Date:
Sat Apr 28 12:09:27 2018 +0000
Revision:
4:aff0722b4e50
Parent:
2:cb6bae534500
Child:
5:695c5531f65e
V0.2

Who changed what in which revision?

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