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@8:73c8188916dc, 2018-05-07 (annotated)

Committer:
wengefa1
Date:
Mon May 07 16:30:51 2018 +0000
Revision:
8:73c8188916dc
Parent:
6:a1fd0f1374e6
Child:
9:5802ccfcc984
es l?uft

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