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@14:0caa7b93af7a, 2018-05-23 (annotated)

Committer:
Alexander_Zuest
Date:
Wed May 23 11:50:58 2018 +0000
Revision:
14:0caa7b93af7a
Parent:
9:5802ccfcc984 
50% Chance for working Routecalculation

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