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@12:811b1364679e, 2018-05-18 (annotated)

Committer:
wengefa1
Date:
Fri May 18 11:36:48 2018 +0000
Revision:
12:811b1364679e
Parent:
6:a1fd0f1374e6 
new mapping

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 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 6:a1fd0f1374e6 51 SDFileSystem sd(PB_5, PB_4, PB_3, PB_10, "sd"); //mosi, miso, sclk, cs
Alexander_Zuest 0:4a0b987c5c94 52
Alexander_Zuest 0:4a0b987c5c94 53 // Pos in route[X,0]
Alexander_Zuest 0:4a0b987c5c94 54
Alexander_Zuest 4:aff0722b4e50 55 const char ZIEL = 0;
Alexander_Zuest 4:aff0722b4e50 56 const char FULLDRIVE = 1;
Alexander_Zuest 4:aff0722b4e50 57 const char TURNRIGHT = 2;
Alexander_Zuest 4:aff0722b4e50 58 const char TURNLEFT = 3;
Alexander_Zuest 4:aff0722b4e50 59 const char PLACETURN90 = 4;
Alexander_Zuest 4:aff0722b4e50 60 const char LEER = 256;
Alexander_Zuest 0:4a0b987c5c94 61
Alexander_Zuest 0:4a0b987c5c94 62
Alexander_Zuest 0:4a0b987c5c94 63 // Pos in route[0,Y]
Alexander_Zuest 0:4a0b987c5c94 64
Alexander_Zuest 0:4a0b987c5c94 65 const int TYPE = 0;
Alexander_Zuest 0:4a0b987c5c94 66 const int LENGHT = 1;
Alexander_Zuest 0:4a0b987c5c94 67
Alexander_Zuest 0:4a0b987c5c94 68 // Codierung Richtungenänderungen
Alexander_Zuest 4:aff0722b4e50 69 const int DREHUNG_LINKS = 1;
Alexander_Zuest 4:aff0722b4e50 70 const int DREHUNG_RECHTS = 2;
Alexander_Zuest 0:4a0b987c5c94 71
Alexander_Zuest 4:aff0722b4e50 72 actionIndex = 1;
Alexander_Zuest 0:4a0b987c5c94 73 int i = 0;
Alexander_Zuest 0:4a0b987c5c94 74
wengefa1 12:811b1364679e 75 int counterY = 0;
wengefa1 12:811b1364679e 76 int counterX = 0;
wengefa1 12:811b1364679e 77 unsigned char a;
Alexander_Zuest 4:aff0722b4e50 78
Alexander_Zuest 0:4a0b987c5c94 79 //SD-Karte lesen
wengefa1 12:811b1364679e 80 FILE *fp = fopen("/sd/map.txt", "r"); // open the file in 'read' mode
wengefa1 12:811b1364679e 81 while (!feof(fp)) { // while not end of file
wengefa1 12:811b1364679e 82 a=fgetc(fp); // get a character/byte from the file
wengefa1 12:811b1364679e 83
wengefa1 12:811b1364679e 84 if(a == 10 || a == 13) {
wengefa1 12:811b1364679e 85 //do nothing
wengefa1 12:811b1364679e 86 } else {
wengefa1 12:811b1364679e 87 map[counterX][counterY] = a;
wengefa1 12:811b1364679e 88 counterX = counterX+1;
wengefa1 12:811b1364679e 89 if(counterX == 20) {
wengefa1 12:811b1364679e 90 counterX = 0;
wengefa1 12:811b1364679e 91 counterY = counterY+1;
wengefa1 12:811b1364679e 92 if(counterY == 10) {
wengefa1 12:811b1364679e 93 break;
Alexander_Zuest 0:4a0b987c5c94 94 }
wengefa1 12:811b1364679e 95
Alexander_Zuest 0:4a0b987c5c94 96 }
wengefa1 12:811b1364679e 97 }
wengefa1 12:811b1364679e 98
Alexander_Zuest 0:4a0b987c5c94 99 }
Alexander_Zuest 0:4a0b987c5c94 100 //-----------------------------------------------------------------------
Alexander_Zuest 0:4a0b987c5c94 101
Alexander_Zuest 4:aff0722b4e50 102 if (map[X-1][ Y-1] == 0){
Alexander_Zuest 5:695c5531f65e 103 route[actionIndex] = PLACETURN90;
Alexander_Zuest 0:4a0b987c5c94 104 direction = 1;
Alexander_Zuest 0:4a0b987c5c94 105 }else{
Alexander_Zuest 5:695c5531f65e 106 route[actionIndex] = FULLDRIVE;
Alexander_Zuest 0:4a0b987c5c94 107 direction = 4;
Alexander_Zuest 0:4a0b987c5c94 108 }
Alexander_Zuest 0:4a0b987c5c94 109
Alexander_Zuest 5:695c5531f65e 110 if(route[actionIndex] == FULLDRIVE){
Alexander_Zuest 4:aff0722b4e50 111 while(map[X-1][Y] == 0){
Alexander_Zuest 0:4a0b987c5c94 112 if(X >= 1 | X <= 19){
Alexander_Zuest 0:4a0b987c5c94 113 X = X -2;
Alexander_Zuest 0:4a0b987c5c94 114 i = i + 1;
Alexander_Zuest 0:4a0b987c5c94 115 }
Alexander_Zuest 0:4a0b987c5c94 116 }
Alexander_Zuest 5:695c5531f65e 117 route[actionIndex+1] = i;
Alexander_Zuest 0:4a0b987c5c94 118 actionIndex = actionIndex +1;
Alexander_Zuest 0:4a0b987c5c94 119
Alexander_Zuest 0:4a0b987c5c94 120 }
Alexander_Zuest 0:4a0b987c5c94 121
Alexander_Zuest 4:aff0722b4e50 122 while (map[X][Y] != 100){ // Ziel = Abbruchbedingung
Alexander_Zuest 0:4a0b987c5c94 123 i = 0;
Alexander_Zuest 0:4a0b987c5c94 124 // ------------------------------------------------------------------------------------------- Grade Strecken fahren
Alexander_Zuest 0:4a0b987c5c94 125 switch (direction){
Alexander_Zuest 4:aff0722b4e50 126 case 1: while(map[X-1][Y-1] == 0){ // Gegen oben
Alexander_Zuest 0:4a0b987c5c94 127 if (Y >= 1 | Y <= 7){
Alexander_Zuest 0:4a0b987c5c94 128 Y = Y - 2;
Alexander_Zuest 0:4a0b987c5c94 129 i = i + 1;
Alexander_Zuest 0:4a0b987c5c94 130 }
Alexander_Zuest 0:4a0b987c5c94 131 }
Alexander_Zuest 0:4a0b987c5c94 132 break;
Alexander_Zuest 0:4a0b987c5c94 133
Alexander_Zuest 4:aff0722b4e50 134 case 2: while(map[X][Y-1] == 0){ // Gegen rechts
Alexander_Zuest 0:4a0b987c5c94 135 if (Y >= 1 | Y <= 19){
Alexander_Zuest 0:4a0b987c5c94 136 X = X + 2;
Alexander_Zuest 0:4a0b987c5c94 137 i = i + 1;
Alexander_Zuest 0:4a0b987c5c94 138 }
Alexander_Zuest 0:4a0b987c5c94 139 }
Alexander_Zuest 0:4a0b987c5c94 140 break;
Alexander_Zuest 0:4a0b987c5c94 141
Alexander_Zuest 5:695c5531f65e 142 case 3: while(map[X][Y] == 0){ // Gegen unten
Alexander_Zuest 0:4a0b987c5c94 143 if (Y >= 1 | Y <= 7){
Alexander_Zuest 0:4a0b987c5c94 144 Y = Y + 2;
Alexander_Zuest 0:4a0b987c5c94 145 i = i + 1;
Alexander_Zuest 0:4a0b987c5c94 146 }
Alexander_Zuest 0:4a0b987c5c94 147 }
Alexander_Zuest 0:4a0b987c5c94 148 break;
Alexander_Zuest 0:4a0b987c5c94 149
Alexander_Zuest 4:aff0722b4e50 150 case 4: while(map[X-1][Y] == 0){ // Gegen rechts
Alexander_Zuest 0:4a0b987c5c94 151 if (Y >= 1 | Y <= 19){
Alexander_Zuest 0:4a0b987c5c94 152 X = X - 2;
Alexander_Zuest 0:4a0b987c5c94 153 i = i + 1;
Alexander_Zuest 0:4a0b987c5c94 154 }
Alexander_Zuest 0:4a0b987c5c94 155 }
Alexander_Zuest 0:4a0b987c5c94 156 break;
Alexander_Zuest 0:4a0b987c5c94 157 }
Alexander_Zuest 5:695c5531f65e 158 actionIndex = actionIndex + 2; // Zur nächstesten Aktion
Alexander_Zuest 5:695c5531f65e 159 route = (char *)realloc(route,(actionIndex+2)*sizeof(char)); // Speicher für neue Aktion initialisieren
Alexander_Zuest 5:695c5531f65e 160 route[actionIndex] = FULLDRIVE; // Fahrmodus auf geradeausfahren
Alexander_Zuest 5:695c5531f65e 161 route[actionIndex+1] = i; // Länge der Strecke eintragen
Alexander_Zuest 0:4a0b987c5c94 162
Alexander_Zuest 0:4a0b987c5c94 163
Alexander_Zuest 0:4a0b987c5c94 164 // ------------------------------------------------------------------------------------------- Drehungen fahren
Alexander_Zuest 0:4a0b987c5c94 165 switch(direction){
Alexander_Zuest 0:4a0b987c5c94 166 case 1: // Roboter gegen oben ausgerichtet
Alexander_Zuest 4:aff0722b4e50 167 if(map[X][Y-1] == 0){ // gegen Rechts abbiegen
Alexander_Zuest 5:695c5531f65e 168 actionIndex = actionIndex + 2; // Zur nächstesten Aktion
Alexander_Zuest 5:695c5531f65e 169 char *route = (char*)realloc(route,(actionIndex+2)*sizeof(char)); // Speicher für neue Aktion initialisieren
Alexander_Zuest 5:695c5531f65e 170 route[actionIndex] = TURNRIGHT;
Alexander_Zuest 5:695c5531f65e 171 route[actionIndex+1] = LEER;
Alexander_Zuest 0:4a0b987c5c94 172 direction = directionControl(DREHUNG_RECHTS, direction);
Alexander_Zuest 0:4a0b987c5c94 173 }
Alexander_Zuest 4:aff0722b4e50 174 if(map[X-1][Y] == 0){ // gegen Links abbiegen
Alexander_Zuest 5:695c5531f65e 175 actionIndex = actionIndex + 2;
Alexander_Zuest 5:695c5531f65e 176 route = (char *)realloc(route,(actionIndex+2)*sizeof(char));
Alexander_Zuest 5:695c5531f65e 177 route[actionIndex]= TURNLEFT;
Alexander_Zuest 5:695c5531f65e 178 route[actionIndex+1] = LEER;
Alexander_Zuest 0:4a0b987c5c94 179 direction = directionControl(DREHUNG_LINKS, direction);
Alexander_Zuest 0:4a0b987c5c94 180 }
Alexander_Zuest 0:4a0b987c5c94 181 case 2: // Roboter gegen rechts ausgerichtet
Alexander_Zuest 4:aff0722b4e50 182 if(map[X-1][Y-1] == 0){ // gegen Oben abbiegen
Alexander_Zuest 5:695c5531f65e 183 actionIndex = actionIndex + 2;
Alexander_Zuest 5:695c5531f65e 184 route = (char *)realloc(route,(actionIndex+2)*sizeof(char));
Alexander_Zuest 5:695c5531f65e 185 route[actionIndex]= TURNLEFT;
Alexander_Zuest 5:695c5531f65e 186 route[actionIndex+1] = LEER;
Alexander_Zuest 0:4a0b987c5c94 187 direction = directionControl(DREHUNG_LINKS, direction);
Alexander_Zuest 0:4a0b987c5c94 188 }
Alexander_Zuest 4:aff0722b4e50 189 if(map[X][Y] == 0){ // gegen Unten abbiegen
Alexander_Zuest 5:695c5531f65e 190 actionIndex = actionIndex + 2;
Alexander_Zuest 5:695c5531f65e 191 route = (char *)realloc(route,(actionIndex+2)*sizeof(char));
Alexander_Zuest 5:695c5531f65e 192 route[actionIndex] = TURNRIGHT;
Alexander_Zuest 5:695c5531f65e 193 route[actionIndex+1] = LEER;
Alexander_Zuest 0:4a0b987c5c94 194 direction = directionControl(DREHUNG_RECHTS, direction);
Alexander_Zuest 0:4a0b987c5c94 195 }
Alexander_Zuest 0:4a0b987c5c94 196 case 3: // Roboter gegen Unten ausgerichtet (Seitenverkehrt)
Alexander_Zuest 4:aff0722b4e50 197 if(map[X][Y-1] == 0){ // gegen Rechts abbiegen
Alexander_Zuest 5:695c5531f65e 198 actionIndex = actionIndex + 2;
Alexander_Zuest 5:695c5531f65e 199 route = (char *)realloc(route,(actionIndex+2)*sizeof(char));
Alexander_Zuest 5:695c5531f65e 200 route[actionIndex] = TURNLEFT;
Alexander_Zuest 5:695c5531f65e 201 route[actionIndex+1] = LEER;
Alexander_Zuest 0:4a0b987c5c94 202 direction = directionControl(DREHUNG_LINKS, direction);
Alexander_Zuest 0:4a0b987c5c94 203 }
Alexander_Zuest 4:aff0722b4e50 204 if(map[X-1][Y] == 0){ // gegen Links abbiegen
Alexander_Zuest 5:695c5531f65e 205 actionIndex = actionIndex + 2;
Alexander_Zuest 5:695c5531f65e 206 route = (char *)realloc(route,(actionIndex+2)*sizeof(char));
Alexander_Zuest 5:695c5531f65e 207 route[actionIndex] = TURNRIGHT;
Alexander_Zuest 5:695c5531f65e 208 route[actionIndex+1] = LEER;
Alexander_Zuest 0:4a0b987c5c94 209 direction = directionControl(DREHUNG_RECHTS, direction);
Alexander_Zuest 0:4a0b987c5c94 210 }
Alexander_Zuest 0:4a0b987c5c94 211 case 4: // Roboter gegen links ausgerichtet
Alexander_Zuest 4:aff0722b4e50 212 if(map[X-1][Y-1] == 0){ // gegen oben abbiegen
Alexander_Zuest 5:695c5531f65e 213 actionIndex = actionIndex + 2;
Alexander_Zuest 5:695c5531f65e 214 route = (char *)realloc(route,(actionIndex+2)*sizeof(char));
Alexander_Zuest 5:695c5531f65e 215 route[actionIndex] = TURNRIGHT;
Alexander_Zuest 5:695c5531f65e 216 route[actionIndex+1] = LEER;
Alexander_Zuest 0:4a0b987c5c94 217 direction = directionControl(DREHUNG_RECHTS, direction);
Alexander_Zuest 0:4a0b987c5c94 218 }
Alexander_Zuest 4:aff0722b4e50 219 if(map[X][Y] == 0){ // gegen unten abbiegen
Alexander_Zuest 5:695c5531f65e 220 actionIndex = actionIndex + 2;
Alexander_Zuest 5:695c5531f65e 221 route = (char *)realloc(route,(actionIndex+2)*sizeof(char));
Alexander_Zuest 5:695c5531f65e 222 route[actionIndex] = TURNLEFT;
Alexander_Zuest 5:695c5531f65e 223 route[actionIndex+1] = LEER;
Alexander_Zuest 0:4a0b987c5c94 224 direction = directionControl(DREHUNG_LINKS, direction);
Alexander_Zuest 0:4a0b987c5c94 225 }
Alexander_Zuest 0:4a0b987c5c94 226 }
Alexander_Zuest 4:aff0722b4e50 227 if (map[X][Y] == 100){
Alexander_Zuest 5:695c5531f65e 228 route = (char *)realloc(route,(actionIndex+2)*sizeof(char));
Alexander_Zuest 5:695c5531f65e 229 actionIndex = actionIndex + 2;
Alexander_Zuest 5:695c5531f65e 230 route[actionIndex] = ZIEL;
Alexander_Zuest 5:695c5531f65e 231 route[actionIndex+1] = ZIEL;
Alexander_Zuest 0:4a0b987c5c94 232 }
Alexander_Zuest 0:4a0b987c5c94 233
Alexander_Zuest 0:4a0b987c5c94 234
Alexander_Zuest 0:4a0b987c5c94 235 } // Ende Kartografierungsschleife
Alexander_Zuest 0:4a0b987c5c94 236
Alexander_Zuest 0:4a0b987c5c94 237 return *route;
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
Alexander_Zuest 0:4a0b987c5c94 245