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@2:cb6bae534500, 2018-04-27 (annotated)

Committer:
Alexander_Zuest
Date:
Fri Apr 27 13:17:39 2018 +0000
Revision:
2:cb6bae534500
Parent:
0:4a0b987c5c94
Child:
4:aff0722b4e50
V0.02

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