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@6:a1fd0f1374e6, 2018-04-30 (annotated)

Committer:
Alexander_Zuest
Date:
Mon Apr 30 13:53:09 2018 +0000
Revision:
6:a1fd0f1374e6
Parent:
5:695c5531f65e
Child:
12:811b1364679e
Child:
14:0caa7b93af7a
V.0.4

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