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

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

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Alexander_Zuest 0:4a0b987c5c94 1 #include "mbed.h"
Alexander_Zuest 0:4a0b987c5c94 2 #include "Controller.h"
Alexander_Zuest 0:4a0b987c5c94 3 #include "MotorDriver.h"
Alexander_Zuest 0:4a0b987c5c94 4 #include "ReadFinalLine.h"
Alexander_Zuest 0:4a0b987c5c94 5 #include "ReadSensor.h"
Alexander_Zuest 0:4a0b987c5c94 6 #include "Mapping.h"
Alexander_Zuest 0:4a0b987c5c94 7
Alexander_Zuest 0:4a0b987c5c94 8 //debug
Alexander_Zuest 0:4a0b987c5c94 9 //DigitalOut myled(LED2);
Alexander_Zuest 0:4a0b987c5c94 10
Alexander_Zuest 0:4a0b987c5c94 11 // Funktionsinitalisierungen
Alexander_Zuest 0:4a0b987c5c94 12 int readSensor(int SensorNummer);// Liesst Ultraschallsensoren 1-3 aus
Alexander_Zuest 0:4a0b987c5c94 13
Alexander_Zuest 0:4a0b987c5c94 14 // int diveOne(); Bewegt Farhzeug um ein Feld nach vorne
Alexander_Zuest 0:4a0b987c5c94 15
Alexander_Zuest 0:4a0b987c5c94 16 int turnRight(int currentDirection); // Biegt Farzeug nach rechts ab und gibt neue Momentanrichtung zurück
Alexander_Zuest 0:4a0b987c5c94 17
Alexander_Zuest 0:4a0b987c5c94 18 int turnLeft(int currentDirection); // Biegt Farzeug nach links ab und gibt neue Momentanrichtung zurück
Alexander_Zuest 0:4a0b987c5c94 19
Alexander_Zuest 0:4a0b987c5c94 20 // int wait(int time); Systemfunktion für Pausen
Alexander_Zuest 0:4a0b987c5c94 21
Alexander_Zuest 0:4a0b987c5c94 22 // SDFileSystem sd(PB_5, PB_4, PB_3, PB_10, "sd"); //mosi, miso, sclk, cs
Alexander_Zuest 0:4a0b987c5c94 23
Alexander_Zuest 0:4a0b987c5c94 24 void mapping(void){ //------------------------------------------------------------------------------------------------------ Mapping
Alexander_Zuest 0:4a0b987c5c94 25
Alexander_Zuest 0:4a0b987c5c94 26 int startPosX = 19; // Feld (10 / 5)
Alexander_Zuest 0:4a0b987c5c94 27 int startPosY = 9;
Alexander_Zuest 0:4a0b987c5c94 28
Alexander_Zuest 0:4a0b987c5c94 29
Alexander_Zuest 0:4a0b987c5c94 30 int map[20][10]; // 20 Zeilen, 10 Spalten, Karte gefüllt mit -1 für keine Daten
Alexander_Zuest 0:4a0b987c5c94 31 int f,g;
Alexander_Zuest 0:4a0b987c5c94 32 for (f = 0; f < 3; f++) {
Alexander_Zuest 0:4a0b987c5c94 33 for (g = 0; g < 5; g++) {
Alexander_Zuest 0:4a0b987c5c94 34 map[f][g] = -1;
Alexander_Zuest 0:4a0b987c5c94 35 }
Alexander_Zuest 0:4a0b987c5c94 36 }
Alexander_Zuest 0:4a0b987c5c94 37
Alexander_Zuest 0:4a0b987c5c94 38 int speed = 1;
Alexander_Zuest 0:4a0b987c5c94 39
Alexander_Zuest 0:4a0b987c5c94 40 int startrichtung = 4;
Alexander_Zuest 0:4a0b987c5c94 41
Alexander_Zuest 0:4a0b987c5c94 42 int P_straight;
Alexander_Zuest 0:4a0b987c5c94 43 int P_rightTurn;
Alexander_Zuest 0:4a0b987c5c94 44 int P_leftTurn;
Alexander_Zuest 0:4a0b987c5c94 45
Alexander_Zuest 0:4a0b987c5c94 46 // Sensoren
Alexander_Zuest 0:4a0b987c5c94 47 int right = 3;
Alexander_Zuest 0:4a0b987c5c94 48 int left = 1;
Alexander_Zuest 0:4a0b987c5c94 49 int front = 2;
Alexander_Zuest 0:4a0b987c5c94 50
Alexander_Zuest 0:4a0b987c5c94 51 // Aktuelle Position
Alexander_Zuest 0:4a0b987c5c94 52 int X = startPosX; // Bei Bewegung: X-2 -> 1 nach oben, X+2 -> 1 nach unten
Alexander_Zuest 0:4a0b987c5c94 53 int Y = startPosY; // Bei Bewegung: Y-2 -> 1 nach links, Y+2 -> 1 nach rechts
Alexander_Zuest 0:4a0b987c5c94 54
Alexander_Zuest 0:4a0b987c5c94 55 // Initalisieren der momentanen Richtung
Alexander_Zuest 0:4a0b987c5c94 56 int currentDirection = startrichtung;
Alexander_Zuest 0:4a0b987c5c94 57
Alexander_Zuest 0:4a0b987c5c94 58 // Verzögerung zwischen Aktionen in ms
Alexander_Zuest 0:4a0b987c5c94 59 int pause = 2;
Alexander_Zuest 0:4a0b987c5c94 60
Alexander_Zuest 0:4a0b987c5c94 61 // Zustandsvariabel während Fahrt = 1 bei Ziel =0
Alexander_Zuest 0:4a0b987c5c94 62 int drive = 1;
Alexander_Zuest 0:4a0b987c5c94 63
Alexander_Zuest 0:4a0b987c5c94 64 int targetReached = 0; // Wird auf 1 gesetzt bei überschrittener Ziellinie sonst 0
Alexander_Zuest 0:4a0b987c5c94 65
Alexander_Zuest 0:4a0b987c5c94 66 // Startinitialisierung, Bestimmen der Abfahrtsrichtung aus dem ersten Feld
Alexander_Zuest 0:4a0b987c5c94 67
Alexander_Zuest 0:4a0b987c5c94 68 if (readSensor(front) == false){ // Gegen vorne freie Fahrt (kein Hindernis -> 0)
Alexander_Zuest 0:4a0b987c5c94 69 map[X-1][Y] = 0;
Alexander_Zuest 0:4a0b987c5c94 70 }else{
Alexander_Zuest 0:4a0b987c5c94 71 map [X-1][Y] = 1;
Alexander_Zuest 0:4a0b987c5c94 72 }
Alexander_Zuest 0:4a0b987c5c94 73
Alexander_Zuest 0:4a0b987c5c94 74 if (readSensor(right) == false){ // Gegen rechts freie Fahrt (kein Hindernis -> 0)
Alexander_Zuest 0:4a0b987c5c94 75 map[X-1][Y-1] = 0;
Alexander_Zuest 0:4a0b987c5c94 76 }else{
Alexander_Zuest 0:4a0b987c5c94 77 map [X-1][Y-1] = 1;
Alexander_Zuest 0:4a0b987c5c94 78 }
Alexander_Zuest 0:4a0b987c5c94 79
Alexander_Zuest 0:4a0b987c5c94 80 map[X][ Y-1] = 1; // ergibt sich aus Feldaufbau und Startrichtung
Alexander_Zuest 0:4a0b987c5c94 81 map[X ][ Y] = 1;
Alexander_Zuest 0:4a0b987c5c94 82
Alexander_Zuest 0:4a0b987c5c94 83
Alexander_Zuest 0:4a0b987c5c94 84 // Erster Schritt
Alexander_Zuest 0:4a0b987c5c94 85
Alexander_Zuest 0:4a0b987c5c94 86 if(map[X-1][Y] == 0){
Alexander_Zuest 0:4a0b987c5c94 87 driveOne(1,speed);
Alexander_Zuest 0:4a0b987c5c94 88 X = X-2;
Alexander_Zuest 0:4a0b987c5c94 89 // debug
Alexander_Zuest 0:4a0b987c5c94 90 printf("\nIf erster schritt\n");
Alexander_Zuest 0:4a0b987c5c94 91 }
Alexander_Zuest 0:4a0b987c5c94 92 else{
Alexander_Zuest 0:4a0b987c5c94 93 currentDirection = turnRight(currentDirection);
Alexander_Zuest 0:4a0b987c5c94 94 wait(pause);
Alexander_Zuest 0:4a0b987c5c94 95 driveOne(1,speed);
Alexander_Zuest 0:4a0b987c5c94 96 Y = Y-2;
Alexander_Zuest 0:4a0b987c5c94 97 printf("\nElse erster schritt\n");
Alexander_Zuest 0:4a0b987c5c94 98 }
Alexander_Zuest 0:4a0b987c5c94 99
Alexander_Zuest 0:4a0b987c5c94 100
Alexander_Zuest 0:4a0b987c5c94 101 // Farhralgorithmus
Alexander_Zuest 0:4a0b987c5c94 102
Alexander_Zuest 0:4a0b987c5c94 103 while (drive == 1){
Alexander_Zuest 0:4a0b987c5c94 104 wait(pause);
Alexander_Zuest 0:4a0b987c5c94 105 // Variabeln zurücksetzen
Alexander_Zuest 0:4a0b987c5c94 106 P_straight = 1;
Alexander_Zuest 0:4a0b987c5c94 107 P_rightTurn = 1;
Alexander_Zuest 0:4a0b987c5c94 108 P_leftTurn = 1;
Alexander_Zuest 0:4a0b987c5c94 109
Alexander_Zuest 0:4a0b987c5c94 110 int s = 12;
Alexander_Zuest 0:4a0b987c5c94 111
Alexander_Zuest 0:4a0b987c5c94 112 // Mapping während Fahrt
Alexander_Zuest 0:4a0b987c5c94 113 switch (currentDirection){
Alexander_Zuest 0:4a0b987c5c94 114 // --------------------------------------------------------------------------- 1 Ausrichtung oben
Alexander_Zuest 0:4a0b987c5c94 115
Alexander_Zuest 0:4a0b987c5c94 116
Alexander_Zuest 0:4a0b987c5c94 117 case 1:
Alexander_Zuest 0:4a0b987c5c94 118
Alexander_Zuest 0:4a0b987c5c94 119 // Kartografierung Feld in Fahrtrichtung
Alexander_Zuest 0:4a0b987c5c94 120 if (readSensor(front) == false){
Alexander_Zuest 0:4a0b987c5c94 121 map[X-1][Y-1] = 0;
Alexander_Zuest 0:4a0b987c5c94 122 P_straight = 0;
Alexander_Zuest 0:4a0b987c5c94 123 }
Alexander_Zuest 0:4a0b987c5c94 124 else{
Alexander_Zuest 0:4a0b987c5c94 125 map[X-1][Y-1] = 1;
Alexander_Zuest 0:4a0b987c5c94 126 }
Alexander_Zuest 0:4a0b987c5c94 127
Alexander_Zuest 0:4a0b987c5c94 128 // Kartografierung Feld rechts
Alexander_Zuest 0:4a0b987c5c94 129 if (readSensor(right) == false){
Alexander_Zuest 0:4a0b987c5c94 130 map[X][Y-1] = 0;
Alexander_Zuest 0:4a0b987c5c94 131 P_rightTurn = 0;
Alexander_Zuest 0:4a0b987c5c94 132 }
Alexander_Zuest 0:4a0b987c5c94 133 else{
Alexander_Zuest 0:4a0b987c5c94 134 map[X][Y-1] = 1;
Alexander_Zuest 0:4a0b987c5c94 135 }
Alexander_Zuest 0:4a0b987c5c94 136
Alexander_Zuest 0:4a0b987c5c94 137 // Kartografierung Feld links
Alexander_Zuest 0:4a0b987c5c94 138
Alexander_Zuest 0:4a0b987c5c94 139 if (readSensor(left) == false){
Alexander_Zuest 0:4a0b987c5c94 140 map[X-1][Y] = 0;
Alexander_Zuest 0:4a0b987c5c94 141 P_leftTurn = 0;
Alexander_Zuest 0:4a0b987c5c94 142 }
Alexander_Zuest 0:4a0b987c5c94 143 else{
Alexander_Zuest 0:4a0b987c5c94 144 map[X-1][Y] = 1;
Alexander_Zuest 0:4a0b987c5c94 145 }
Alexander_Zuest 0:4a0b987c5c94 146
Alexander_Zuest 0:4a0b987c5c94 147 // Kartografierung Feld nach hinten
Alexander_Zuest 0:4a0b987c5c94 148 map[X][Y] = 1;
Alexander_Zuest 0:4a0b987c5c94 149 // debug
Alexander_Zuest 0:4a0b987c5c94 150 printf("case 1 \n");
Alexander_Zuest 0:4a0b987c5c94 151 break;
Alexander_Zuest 0:4a0b987c5c94 152
Alexander_Zuest 0:4a0b987c5c94 153 // --------------------------------------------------------------------------- 2 Ausrichtung Rechts
Alexander_Zuest 0:4a0b987c5c94 154 case 2:
Alexander_Zuest 0:4a0b987c5c94 155
Alexander_Zuest 0:4a0b987c5c94 156 // Kartografierung Feld in Fahrtrichtung
Alexander_Zuest 0:4a0b987c5c94 157 if (readSensor(front) == false){
Alexander_Zuest 0:4a0b987c5c94 158 map[X][Y-1] = 0;
Alexander_Zuest 0:4a0b987c5c94 159 P_straight = 0;
Alexander_Zuest 0:4a0b987c5c94 160 }
Alexander_Zuest 0:4a0b987c5c94 161 else{
Alexander_Zuest 0:4a0b987c5c94 162 map[X][Y-1] = 1;
Alexander_Zuest 0:4a0b987c5c94 163 }
Alexander_Zuest 0:4a0b987c5c94 164
Alexander_Zuest 0:4a0b987c5c94 165 // Kartografierung Feld rechts
Alexander_Zuest 0:4a0b987c5c94 166 if (readSensor(right) == false){
Alexander_Zuest 0:4a0b987c5c94 167 map[X][Y] = 0;
Alexander_Zuest 0:4a0b987c5c94 168 P_rightTurn = 0;
Alexander_Zuest 0:4a0b987c5c94 169 }
Alexander_Zuest 0:4a0b987c5c94 170 else{
Alexander_Zuest 0:4a0b987c5c94 171 map[X][Y] = 1;
Alexander_Zuest 0:4a0b987c5c94 172 }
Alexander_Zuest 0:4a0b987c5c94 173
Alexander_Zuest 0:4a0b987c5c94 174 // Kartografierung Feld links
Alexander_Zuest 0:4a0b987c5c94 175
Alexander_Zuest 0:4a0b987c5c94 176 if (readSensor(left) == false){
Alexander_Zuest 0:4a0b987c5c94 177 map[X-1][Y-1] = 0;
Alexander_Zuest 0:4a0b987c5c94 178 P_leftTurn = 0;
Alexander_Zuest 0:4a0b987c5c94 179 }
Alexander_Zuest 0:4a0b987c5c94 180 else{
Alexander_Zuest 0:4a0b987c5c94 181 map[X-1][Y-1] = 1;
Alexander_Zuest 0:4a0b987c5c94 182 }
Alexander_Zuest 0:4a0b987c5c94 183
Alexander_Zuest 0:4a0b987c5c94 184 // Kartografierung Feld nach hinten
Alexander_Zuest 0:4a0b987c5c94 185 map[X-1][Y] = 1;
Alexander_Zuest 0:4a0b987c5c94 186 // debug
Alexander_Zuest 0:4a0b987c5c94 187 printf("case 2 \n");
Alexander_Zuest 0:4a0b987c5c94 188 break;
Alexander_Zuest 0:4a0b987c5c94 189 // --------------------------------------------------------------------------- 3 Ausrichtung Unten
Alexander_Zuest 0:4a0b987c5c94 190 case 3:
Alexander_Zuest 0:4a0b987c5c94 191
Alexander_Zuest 0:4a0b987c5c94 192 // Kartografierung Feld in Fahrtrichtung
Alexander_Zuest 0:4a0b987c5c94 193 if (readSensor(front) == false){
Alexander_Zuest 0:4a0b987c5c94 194 map[X][Y] = 0;
Alexander_Zuest 0:4a0b987c5c94 195 P_straight = 0;
Alexander_Zuest 0:4a0b987c5c94 196 }
Alexander_Zuest 0:4a0b987c5c94 197 else{
Alexander_Zuest 0:4a0b987c5c94 198 map[X][Y] = 1;
Alexander_Zuest 0:4a0b987c5c94 199 }
Alexander_Zuest 0:4a0b987c5c94 200
Alexander_Zuest 0:4a0b987c5c94 201 // Kartografierung Feld rechts
Alexander_Zuest 0:4a0b987c5c94 202 if (readSensor(right) == false){
Alexander_Zuest 0:4a0b987c5c94 203 map[X-1][Y] = 0;
Alexander_Zuest 0:4a0b987c5c94 204 P_rightTurn = 0;
Alexander_Zuest 0:4a0b987c5c94 205 }
Alexander_Zuest 0:4a0b987c5c94 206 else{
Alexander_Zuest 0:4a0b987c5c94 207 map[X-1][Y] = 1;
Alexander_Zuest 0:4a0b987c5c94 208 }
Alexander_Zuest 0:4a0b987c5c94 209
Alexander_Zuest 0:4a0b987c5c94 210 // Kartografierung Feld links
Alexander_Zuest 0:4a0b987c5c94 211
Alexander_Zuest 0:4a0b987c5c94 212 if (readSensor(left) == false){
Alexander_Zuest 0:4a0b987c5c94 213 map[X][Y-1] = 0;
Alexander_Zuest 0:4a0b987c5c94 214 P_leftTurn = 0;
Alexander_Zuest 0:4a0b987c5c94 215 }
Alexander_Zuest 0:4a0b987c5c94 216 else{
Alexander_Zuest 0:4a0b987c5c94 217 map[X][Y-1] = 1;
Alexander_Zuest 0:4a0b987c5c94 218 }
Alexander_Zuest 0:4a0b987c5c94 219
Alexander_Zuest 0:4a0b987c5c94 220 // Kartografierung Feld nach hinten
Alexander_Zuest 0:4a0b987c5c94 221 map[X-1][Y-1] = 1;
Alexander_Zuest 0:4a0b987c5c94 222 // debug
Alexander_Zuest 0:4a0b987c5c94 223 printf("case 3: S = %d\n",s);
Alexander_Zuest 0:4a0b987c5c94 224 break;
Alexander_Zuest 0:4a0b987c5c94 225 // --------------------------------------------------------------------------- 4 Ausrichtung Links
Alexander_Zuest 0:4a0b987c5c94 226 case 4:
Alexander_Zuest 0:4a0b987c5c94 227
Alexander_Zuest 0:4a0b987c5c94 228 // Kartografierung Feld in Fahrtrichtung
Alexander_Zuest 0:4a0b987c5c94 229 if (readSensor(front) == false){
Alexander_Zuest 0:4a0b987c5c94 230 map[X-1][Y] = 0;
Alexander_Zuest 0:4a0b987c5c94 231 P_straight = 0;
Alexander_Zuest 0:4a0b987c5c94 232 }
Alexander_Zuest 0:4a0b987c5c94 233 else{
Alexander_Zuest 0:4a0b987c5c94 234 map[X-1][Y] = 1;
Alexander_Zuest 0:4a0b987c5c94 235 }
Alexander_Zuest 0:4a0b987c5c94 236
Alexander_Zuest 0:4a0b987c5c94 237 // Kartografierung Feld rechts
Alexander_Zuest 0:4a0b987c5c94 238 if (readSensor(right) == false){
Alexander_Zuest 0:4a0b987c5c94 239 map[X-1][Y-1] = 0;
Alexander_Zuest 0:4a0b987c5c94 240 P_rightTurn = 0;
Alexander_Zuest 0:4a0b987c5c94 241 }
Alexander_Zuest 0:4a0b987c5c94 242 else{
Alexander_Zuest 0:4a0b987c5c94 243 map[X-1][Y-1] = 1;
Alexander_Zuest 0:4a0b987c5c94 244 }
Alexander_Zuest 0:4a0b987c5c94 245
Alexander_Zuest 0:4a0b987c5c94 246 // Kartografierung Feld links
Alexander_Zuest 0:4a0b987c5c94 247
Alexander_Zuest 0:4a0b987c5c94 248 if (readSensor(left) == false){
Alexander_Zuest 0:4a0b987c5c94 249 map[X][Y] = 0;
Alexander_Zuest 0:4a0b987c5c94 250 P_leftTurn = 0;
Alexander_Zuest 0:4a0b987c5c94 251 }
Alexander_Zuest 0:4a0b987c5c94 252 else{
Alexander_Zuest 0:4a0b987c5c94 253 map[X][Y] = 1;
Alexander_Zuest 0:4a0b987c5c94 254 }
Alexander_Zuest 0:4a0b987c5c94 255
Alexander_Zuest 0:4a0b987c5c94 256 // Kartografierung Feld nach hinten
Alexander_Zuest 0:4a0b987c5c94 257 map[X-1][Y-1] = 1;
Alexander_Zuest 0:4a0b987c5c94 258 // debug
Alexander_Zuest 0:4a0b987c5c94 259 printf("case 4\n");
Alexander_Zuest 0:4a0b987c5c94 260
Alexander_Zuest 0:4a0b987c5c94 261 break;
Alexander_Zuest 0:4a0b987c5c94 262 default: printf("Error: Direction out of definition\n");
Alexander_Zuest 0:4a0b987c5c94 263 }
Alexander_Zuest 0:4a0b987c5c94 264 // debug
Alexander_Zuest 0:4a0b987c5c94 265 if (P_straight == 0){
Alexander_Zuest 0:4a0b987c5c94 266 printf("Geradeaus frei");
Alexander_Zuest 0:4a0b987c5c94 267 }
Alexander_Zuest 0:4a0b987c5c94 268 if (P_rightTurn == 0){
Alexander_Zuest 0:4a0b987c5c94 269 printf("Rechts frei");
Alexander_Zuest 0:4a0b987c5c94 270 }
Alexander_Zuest 0:4a0b987c5c94 271 if (P_leftTurn == 0){
Alexander_Zuest 0:4a0b987c5c94 272 printf("Links frei");
Alexander_Zuest 0:4a0b987c5c94 273 }
Alexander_Zuest 0:4a0b987c5c94 274 //printf("Gerade %d\nRechts %d\nLinks %d\n", P_straight, P_rightTurn, P_leftTurn );
Alexander_Zuest 0:4a0b987c5c94 275
Alexander_Zuest 0:4a0b987c5c94 276 printf("Momentane Richtung = %d\n", currentDirection );
Alexander_Zuest 0:4a0b987c5c94 277
Alexander_Zuest 0:4a0b987c5c94 278
Alexander_Zuest 0:4a0b987c5c94 279 // Fahren gem. Rechtsfahralgorithmus
Alexander_Zuest 0:4a0b987c5c94 280
Alexander_Zuest 0:4a0b987c5c94 281 if (P_rightTurn == 0) {
Alexander_Zuest 0:4a0b987c5c94 282 currentDirection = turnRight(currentDirection);
Alexander_Zuest 0:4a0b987c5c94 283 //wait(pause);
Alexander_Zuest 0:4a0b987c5c94 284 printf("Ich biege rechts ab\n");
Alexander_Zuest 0:4a0b987c5c94 285 driveOne(1,speed);
Alexander_Zuest 0:4a0b987c5c94 286 //myled = 0;
Alexander_Zuest 0:4a0b987c5c94 287 } else{
Alexander_Zuest 0:4a0b987c5c94 288 if (P_straight == 0){
Alexander_Zuest 0:4a0b987c5c94 289 driveOne(1,speed);
Alexander_Zuest 0:4a0b987c5c94 290 printf("Ich fahre gerade aus\n");
Alexander_Zuest 0:4a0b987c5c94 291 //myled = 0;
Alexander_Zuest 0:4a0b987c5c94 292 } else{
Alexander_Zuest 0:4a0b987c5c94 293 if(P_leftTurn == 0){
Alexander_Zuest 0:4a0b987c5c94 294 currentDirection = turnLeft(currentDirection);
Alexander_Zuest 0:4a0b987c5c94 295 //wait(pause);
Alexander_Zuest 0:4a0b987c5c94 296 printf("Ich biege links ab\n");
Alexander_Zuest 0:4a0b987c5c94 297 driveOne(1,speed);
Alexander_Zuest 0:4a0b987c5c94 298 //myled = 0;
Alexander_Zuest 0:4a0b987c5c94 299 } else{
Alexander_Zuest 0:4a0b987c5c94 300 printf("Ich wende!!!\n");
Alexander_Zuest 0:4a0b987c5c94 301 currentDirection = turnRight(currentDirection);
Alexander_Zuest 0:4a0b987c5c94 302 //wait(pause);
Alexander_Zuest 0:4a0b987c5c94 303 currentDirection = turnRight(currentDirection);
Alexander_Zuest 0:4a0b987c5c94 304 //wait(pause);
Alexander_Zuest 0:4a0b987c5c94 305 driveOne(1,speed);
Alexander_Zuest 0:4a0b987c5c94 306 //myled = 1;
Alexander_Zuest 0:4a0b987c5c94 307 }
Alexander_Zuest 0:4a0b987c5c94 308 }
Alexander_Zuest 0:4a0b987c5c94 309 }
Alexander_Zuest 0:4a0b987c5c94 310
Alexander_Zuest 0:4a0b987c5c94 311 printf("Neue Richtung = %d\n", currentDirection );
Alexander_Zuest 0:4a0b987c5c94 312 // Aktualisieren der Position:
Alexander_Zuest 0:4a0b987c5c94 313
Alexander_Zuest 0:4a0b987c5c94 314 switch (currentDirection){
Alexander_Zuest 0:4a0b987c5c94 315 case 1:
Alexander_Zuest 0:4a0b987c5c94 316 Y = Y-2;
Alexander_Zuest 0:4a0b987c5c94 317 break;
Alexander_Zuest 0:4a0b987c5c94 318 //----------
Alexander_Zuest 0:4a0b987c5c94 319 case 2:
Alexander_Zuest 0:4a0b987c5c94 320 X = X+2;
Alexander_Zuest 0:4a0b987c5c94 321 break;
Alexander_Zuest 0:4a0b987c5c94 322 //----------
Alexander_Zuest 0:4a0b987c5c94 323 case 3:
Alexander_Zuest 0:4a0b987c5c94 324 Y = Y+2;
Alexander_Zuest 0:4a0b987c5c94 325 break;
Alexander_Zuest 0:4a0b987c5c94 326 //----------
Alexander_Zuest 0:4a0b987c5c94 327 case 4:
Alexander_Zuest 0:4a0b987c5c94 328 X = X-2;
Alexander_Zuest 0:4a0b987c5c94 329 break;
Alexander_Zuest 0:4a0b987c5c94 330 }
Alexander_Zuest 0:4a0b987c5c94 331
Alexander_Zuest 0:4a0b987c5c94 332 // Bei Ueberfahren der Ziellinie
Alexander_Zuest 0:4a0b987c5c94 333 if(targetReached == true){ // Prüft Linienüberfahrt
Alexander_Zuest 0:4a0b987c5c94 334 drive = 0; // Beendet Schlafendurchlauf
Alexander_Zuest 0:4a0b987c5c94 335 }
Alexander_Zuest 0:4a0b987c5c94 336
Alexander_Zuest 0:4a0b987c5c94 337 // Verhalten bei Schlaufenfahren = Bereits befahrenen Feldern
Alexander_Zuest 0:4a0b987c5c94 338
Alexander_Zuest 0:4a0b987c5c94 339 // Setzt in Richtung des letzten Feldes eine Wand
Alexander_Zuest 0:4a0b987c5c94 340 if (map[X][Y] != -1){
Alexander_Zuest 0:4a0b987c5c94 341 switch (currentDirection){
Alexander_Zuest 0:4a0b987c5c94 342 case 1:
Alexander_Zuest 0:4a0b987c5c94 343 map[X][Y] = 1;
Alexander_Zuest 0:4a0b987c5c94 344 break;
Alexander_Zuest 0:4a0b987c5c94 345
Alexander_Zuest 0:4a0b987c5c94 346 case 2:
Alexander_Zuest 0:4a0b987c5c94 347 map[X-1][Y] = 1;
Alexander_Zuest 0:4a0b987c5c94 348 break;
Alexander_Zuest 0:4a0b987c5c94 349
Alexander_Zuest 0:4a0b987c5c94 350 case 3:
Alexander_Zuest 0:4a0b987c5c94 351 map[X-1][Y-1] = 1;
Alexander_Zuest 0:4a0b987c5c94 352 break;
Alexander_Zuest 0:4a0b987c5c94 353
Alexander_Zuest 0:4a0b987c5c94 354 case 4:
Alexander_Zuest 0:4a0b987c5c94 355 map[X][Y-1] = 1;
Alexander_Zuest 0:4a0b987c5c94 356 break;
Alexander_Zuest 0:4a0b987c5c94 357 }
Alexander_Zuest 0:4a0b987c5c94 358
Alexander_Zuest 0:4a0b987c5c94 359 }
Alexander_Zuest 0:4a0b987c5c94 360
Alexander_Zuest 0:4a0b987c5c94 361 } // Ende Fahrschlaufe
Alexander_Zuest 0:4a0b987c5c94 362
Alexander_Zuest 0:4a0b987c5c94 363 // Zielmanoever ausführen-------------------------------------------------------------------------------------
Alexander_Zuest 0:4a0b987c5c94 364
Alexander_Zuest 0:4a0b987c5c94 365 map[X][Y] = 100;
Alexander_Zuest 0:4a0b987c5c94 366 map[X-1][Y] = 100;
Alexander_Zuest 0:4a0b987c5c94 367 map[X][Y-1] = 100;
Alexander_Zuest 0:4a0b987c5c94 368 map[X-1][Y-1] = 100;
Alexander_Zuest 0:4a0b987c5c94 369
Alexander_Zuest 0:4a0b987c5c94 370 // Karte auf SD speichern-------------------------------------------------------------------------------------
Alexander_Zuest 0:4a0b987c5c94 371
Alexander_Zuest 0:4a0b987c5c94 372 /*
Alexander_Zuest 0:4a0b987c5c94 373 //Mount the filesystem
Alexander_Zuest 0:4a0b987c5c94 374
Alexander_Zuest 0:4a0b987c5c94 375 sd.mount();
Alexander_Zuest 0:4a0b987c5c94 376 int i;
Alexander_Zuest 0:4a0b987c5c94 377 int g;
Alexander_Zuest 0:4a0b987c5c94 378
Alexander_Zuest 0:4a0b987c5c94 379 //Perform a write test
Alexander_Zuest 0:4a0b987c5c94 380 printf("\nWriting to SD card...");
Alexander_Zuest 0:4a0b987c5c94 381 FILE *fp = fopen("/sd/map.csv", "w");
Alexander_Zuest 0:4a0b987c5c94 382 if (fp != NULL) {
Alexander_Zuest 0:4a0b987c5c94 383 for (i = 0; i < 9; i++){
Alexander_Zuest 0:4a0b987c5c94 384 for (g = 0; g < 19; g++){
Alexander_Zuest 0:4a0b987c5c94 385 fprintf(fp, "%d,",map[g,i]);
Alexander_Zuest 0:4a0b987c5c94 386 }
Alexander_Zuest 0:4a0b987c5c94 387 fprintf(fp,"\n\r")
Alexander_Zuest 0:4a0b987c5c94 388 }
Alexander_Zuest 0:4a0b987c5c94 389 printf("Matrix has been sucessfully saved in map.csv\n\r");
Alexander_Zuest 0:4a0b987c5c94 390 } else {
Alexander_Zuest 0:4a0b987c5c94 391 printf("Save-process failed!\n\r");
Alexander_Zuest 0:4a0b987c5c94 392 }
Alexander_Zuest 0:4a0b987c5c94 393
Alexander_Zuest 0:4a0b987c5c94 394 //Unmount the filesystem
Alexander_Zuest 0:4a0b987c5c94 395 sd.unmount();
Alexander_Zuest 0:4a0b987c5c94 396 */
Alexander_Zuest 0:4a0b987c5c94 397 }
Alexander_Zuest 0:4a0b987c5c94 398
Alexander_Zuest 0:4a0b987c5c94 399
Alexander_Zuest 0:4a0b987c5c94 400
Alexander_Zuest 0:4a0b987c5c94 401
Alexander_Zuest 0:4a0b987c5c94 402
Alexander_Zuest 0:4a0b987c5c94 403
Alexander_Zuest 0:4a0b987c5c94 404
Alexander_Zuest 0:4a0b987c5c94 405
Alexander_Zuest 0:4a0b987c5c94 406
Alexander_Zuest 0:4a0b987c5c94 407
Alexander_Zuest 0:4a0b987c5c94 408
Alexander_Zuest 0:4a0b987c5c94 409