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@11:68ee67d17320, 2018-05-12 (annotated)

Committer:
wengefa1
Date:
Sat May 12 12:19:26 2018 +0000
Revision:
11:68ee67d17320
Parent:
9:5802ccfcc984
Child:
12:811b1364679e
regelung an rechter Wand eingef?gt;

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