PES2_R2D2.0 / Mbed 2 deprecated MicroMouse_MASTER_FOUR

Pathfinding nach rechts funktioniert noch nicht...der rest schon

Dependencies:   mbed

Fork of MicroMouse_MASTER_THREE by PES2_R2D2.0

main.cpp@9:ab19796bf14a, 2018-05-16 (annotated)

Committer:
ruesipat
Date:
Wed May 16 16:41:44 2018 +0000
Revision:
9:ab19796bf14a
Parent:
8:1c8a747c49c8 
;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ruesipat 0:a9fe4ef404bf 1 #include "mbed.h"
ruesipat 1:d9e840c48b1e 2
ruesipat 0:a9fe4ef404bf 3 #include "IRSensor.h"
ruesipat 0:a9fe4ef404bf 4 #include "EncoderCounter.h"
ruesipat 0:a9fe4ef404bf 5 #include "LowpassFilter.h"
ruesipat 0:a9fe4ef404bf 6 #include "Controller.h"
ruesipat 1:d9e840c48b1e 7 #include "KontrastSensor.h"
ruesipat 1:d9e840c48b1e 8 #include "Drive.h"
ruesipat 1:d9e840c48b1e 9 #include "CheckWalls.h"
ruesipat 1:d9e840c48b1e 10 #include "Turn.h"
ruesipat 2:592f01278db4 11
TheDarkDurzo 8:1c8a747c49c8 12 Serial pc(USBTX,USBRX,460800);
TheDarkDurzo 8:1c8a747c49c8 13
ruesipat 9:ab19796bf14a 14 //Initialisierung LED/Button
ruesipat 1:d9e840c48b1e 15 DigitalIn button(USER_BUTTON); //Moduswählknopf
ruesipat 1:d9e840c48b1e 16 DigitalOut myled(LED1); //Heartbeat (evt auch Anzeige für Modus, vor start
ruesipat 1:d9e840c48b1e 17
ruesipat 1:d9e840c48b1e 18
ruesipat 1:d9e840c48b1e 19 //Initialisierung IR-Sensoren
ruesipat 1:d9e840c48b1e 20 AnalogIn distance0(PC_2); //Kreieren der Eingangsobjekte
ruesipat 1:d9e840c48b1e 21 AnalogIn distance1(PC_3);
ruesipat 1:d9e840c48b1e 22 AnalogIn distance2(PC_5);
ruesipat 1:d9e840c48b1e 23 AnalogIn distance3(PB_1);
ruesipat 1:d9e840c48b1e 24 IRSensor irSensor0(distance0); //rechts
ruesipat 1:d9e840c48b1e 25 IRSensor irSensor1(distance1); //vorne
ruesipat 1:d9e840c48b1e 26 IRSensor irSensor2(distance2); //links-vorne
ruesipat 5:b8b1a979b0d5 27 IRSensor irSensor3(distance3); //links-hinten
ruesipat 1:d9e840c48b1e 28
ruesipat 1:d9e840c48b1e 29 //Initialisierung Kontrastsensor
ruesipat 1:d9e840c48b1e 30 AnalogIn kontrast(PC_0);
ruesipat 1:d9e840c48b1e 31
ruesipat 1:d9e840c48b1e 32 //Initialisierung Motor und Encoder
ruesipat 0:a9fe4ef404bf 33 DigitalOut enableMotorDriver(PB_2);
ruesipat 1:d9e840c48b1e 34 PwmOut pwmLeft(PA_8); //Motor links
ruesipat 1:d9e840c48b1e 35 PwmOut pwmRight(PA_9); //Motor rechts
ruesipat 1:d9e840c48b1e 36 EncoderCounter counterLeft(PB_6, PB_7); //Encoder für Motor links
ruesipat 1:d9e840c48b1e 37 EncoderCounter counterRight(PA_6, PC_7);//Encoder für Motor rechts
ruesipat 0:a9fe4ef404bf 38
ruesipat 0:a9fe4ef404bf 39 DigitalOut enable(PC_1);
ruesipat 0:a9fe4ef404bf 40
ruesipat 1:d9e840c48b1e 41 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
ruesipat 0:a9fe4ef404bf 42
ruesipat 1:d9e840c48b1e 43 int state = 0;
TheDarkDurzo 8:1c8a747c49c8 44 int mode = 1;
TheDarkDurzo 8:1c8a747c49c8 45
ruesipat 1:d9e840c48b1e 46
ruesipat 1:d9e840c48b1e 47 int main()
ruesipat 1:d9e840c48b1e 48 {
ruesipat 5:b8b1a979b0d5 49 enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
ruesipat 5:b8b1a979b0d5 50 enable = 1;
ruesipat 5:b8b1a979b0d5 51
ruesipat 4:e3f388933954 52 int wallRight = 0;
ruesipat 4:e3f388933954 53 int wallFront = 0;
ruesipat 5:b8b1a979b0d5 54 int wallLeft = 0;
ruesipat 4:e3f388933954 55 int blackLine = 0;
ruesipat 7:5ef09519a6e9 56 int dontStop = 0;
ruesipat 9:ab19796bf14a 57 int lab[5][10];
ruesipat 9:ab19796bf14a 58 int path = 0;
ruesipat 9:ab19796bf14a 59 int pathNext = 0;
ruesipat 9:ab19796bf14a 60 int pathArr[50];
ruesipat 9:ab19796bf14a 61 int directions[50];
ruesipat 9:ab19796bf14a 62 int modeStart = 0;
ruesipat 9:ab19796bf14a 63 int startx = 0;
ruesipat 9:ab19796bf14a 64 int starty = 10;
ruesipat 9:ab19796bf14a 65 int ausr = 1;
ruesipat 9:ab19796bf14a 66 int c = 0;
ruesipat 9:ab19796bf14a 67 int firstMove = 0;
ruesipat 9:ab19796bf14a 68
ruesipat 7:5ef09519a6e9 69 //Funktionsdeklarationen
ruesipat 9:ab19796bf14a 70
ruesipat 5:b8b1a979b0d5 71 CheckWalls checkWalls(irSensor0, irSensor1, irSensor2, wallRight, wallFront, wallLeft); //Ermittlung wo freie Wege(0) bzw welche Waende vorhanden (1)
TheDarkDurzo 8:1c8a747c49c8 72 Turn turn(counterLeft, counterRight, controller, wallRight, wallFront, wallLeft, dontStop, modeStart, path); //Nach Ausrichtung bewegt sich der Roboter um ein Feld weiter und die Ausrichutung beginnt von vorne
ruesipat 5:b8b1a979b0d5 73 KontrastSensor kontrastSensor(kontrast, blackLine);
ruesipat 9:ab19796bf14a 74 Drive drive(kontrastSensor, counterLeft, counterRight, controller, irSensor0, irSensor1, irSensor2, irSensor3, dontStop, modeStart, path, pathNext); //20cm nach vorne...//Ablauf ist jeweils immer Feldweise...Waende checken, allenfalls drehen (Ausrichten), waehrend der Fahrt nur Ausrichtung der Geradeausfahrt...
ruesipat 0:a9fe4ef404bf 75
ruesipat 2:592f01278db4 76
ruesipat 1:d9e840c48b1e 77 while(1) { // Wiederholungsschleife
ruesipat 9:ab19796bf14a 78
ruesipat 5:b8b1a979b0d5 79 switch(state) { //Wartemodus 1. Lauf
ruesipat 5:b8b1a979b0d5 80 case 0: {
ruesipat 5:b8b1a979b0d5 81 while (button) {
ruesipat 5:b8b1a979b0d5 82 wait(1.0f); //Die Ruhe vor dem Sturm...
ruesipat 5:b8b1a979b0d5 83 }
ruesipat 9:ab19796bf14a 84 firstMove = 1;
ruesipat 5:b8b1a979b0d5 85 state = 1;
ruesipat 9:ab19796bf14a 86
ruesipat 5:b8b1a979b0d5 87 break;
ruesipat 5:b8b1a979b0d5 88 }
ruesipat 5:b8b1a979b0d5 89 case 1: {// 1.Lauf Fahrzyklus und Aufzeichungszyklus
ruesipat 9:ab19796bf14a 90
ruesipat 5:b8b1a979b0d5 91 checkWalls.check(); //prueft wo Waende vorhanden sind
ruesipat 9:ab19796bf14a 92
ruesipat 5:b8b1a979b0d5 93
ruesipat 9:ab19796bf14a 94
TheDarkDurzo 8:1c8a747c49c8 95 turn.turning();
ruesipat 9:ab19796bf14a 96
ruesipat 9:ab19796bf14a 97 if(firstMove == 1){
ruesipat 9:ab19796bf14a 98 dontStop = 1;
ruesipat 9:ab19796bf14a 99 firstMove = 0;
ruesipat 9:ab19796bf14a 100
ruesipat 9:ab19796bf14a 101 /* **************************** */
ruesipat 9:ab19796bf14a 102 /* Anpassung 16.02.18 */
ruesipat 9:ab19796bf14a 103 if (wallFront == 0) {
ruesipat 9:ab19796bf14a 104 ausr = 1;
ruesipat 9:ab19796bf14a 105 lab[startx][starty] = ausr;
ruesipat 9:ab19796bf14a 106 starty--;
ruesipat 9:ab19796bf14a 107 }else if (wallRight == 0) {
ruesipat 9:ab19796bf14a 108 ausr = 3; // Ausrichtung zu kontrollieren!!!!
ruesipat 9:ab19796bf14a 109 lab[startx][starty] = ausr;
ruesipat 9:ab19796bf14a 110 startx++;
ruesipat 9:ab19796bf14a 111 }
ruesipat 9:ab19796bf14a 112 /***********************/
ruesipat 9:ab19796bf14a 113 }
ruesipat 9:ab19796bf14a 114
ruesipat 9:ab19796bf14a 115
ruesipat 9:ab19796bf14a 116 //merkt sich wie sich die Ausrichtung des Fahrzeugs im Labyrinth veraendert und speichert diese in ein Array
ruesipat 9:ab19796bf14a 117
TheDarkDurzo 8:1c8a747c49c8 118 if(wallLeft == 0) {
TheDarkDurzo 8:1c8a747c49c8 119 switch(ausr) {
TheDarkDurzo 8:1c8a747c49c8 120 case 1: {
TheDarkDurzo 8:1c8a747c49c8 121 ausr = 2;
TheDarkDurzo 8:1c8a747c49c8 122 lab[startx][starty] = ausr;
TheDarkDurzo 8:1c8a747c49c8 123 startx--;
TheDarkDurzo 8:1c8a747c49c8 124 break;
TheDarkDurzo 8:1c8a747c49c8 125 }
TheDarkDurzo 8:1c8a747c49c8 126 case 2: {
TheDarkDurzo 8:1c8a747c49c8 127 ausr = 4;
TheDarkDurzo 8:1c8a747c49c8 128 lab[startx][starty] = ausr;
TheDarkDurzo 8:1c8a747c49c8 129 starty++;
TheDarkDurzo 8:1c8a747c49c8 130 break;
TheDarkDurzo 8:1c8a747c49c8 131 }
TheDarkDurzo 8:1c8a747c49c8 132 case 3: {
TheDarkDurzo 8:1c8a747c49c8 133 ausr = 1;
TheDarkDurzo 8:1c8a747c49c8 134 lab[startx][starty] = ausr;
TheDarkDurzo 8:1c8a747c49c8 135 starty--;
TheDarkDurzo 8:1c8a747c49c8 136 break;
TheDarkDurzo 8:1c8a747c49c8 137 }
TheDarkDurzo 8:1c8a747c49c8 138 case 4: {
TheDarkDurzo 8:1c8a747c49c8 139 ausr = 3;
TheDarkDurzo 8:1c8a747c49c8 140 lab[startx][starty] = ausr;
TheDarkDurzo 8:1c8a747c49c8 141 startx++;
TheDarkDurzo 8:1c8a747c49c8 142 break;
TheDarkDurzo 8:1c8a747c49c8 143 }
TheDarkDurzo 8:1c8a747c49c8 144 }
TheDarkDurzo 8:1c8a747c49c8 145
TheDarkDurzo 8:1c8a747c49c8 146 } else if(wallFront == 0) {
TheDarkDurzo 8:1c8a747c49c8 147 switch(ausr) {
TheDarkDurzo 8:1c8a747c49c8 148 case 1: {
TheDarkDurzo 8:1c8a747c49c8 149 ausr = 1;
TheDarkDurzo 8:1c8a747c49c8 150 lab[startx][starty] = ausr;
TheDarkDurzo 8:1c8a747c49c8 151 starty--;
TheDarkDurzo 8:1c8a747c49c8 152 break;
TheDarkDurzo 8:1c8a747c49c8 153 }
TheDarkDurzo 8:1c8a747c49c8 154 case 2: {
TheDarkDurzo 8:1c8a747c49c8 155 ausr = 2;
TheDarkDurzo 8:1c8a747c49c8 156 lab[startx][starty] = ausr;
TheDarkDurzo 8:1c8a747c49c8 157 startx--;
TheDarkDurzo 8:1c8a747c49c8 158 break;
TheDarkDurzo 8:1c8a747c49c8 159 }
TheDarkDurzo 8:1c8a747c49c8 160 case 3: {
TheDarkDurzo 8:1c8a747c49c8 161 ausr = 3;
TheDarkDurzo 8:1c8a747c49c8 162 lab[startx][starty] = ausr;
TheDarkDurzo 8:1c8a747c49c8 163 startx++;
TheDarkDurzo 8:1c8a747c49c8 164 break;
TheDarkDurzo 8:1c8a747c49c8 165 }
TheDarkDurzo 8:1c8a747c49c8 166 case 4: {
TheDarkDurzo 8:1c8a747c49c8 167 ausr = 4;
TheDarkDurzo 8:1c8a747c49c8 168 lab[startx][starty] = ausr;
TheDarkDurzo 8:1c8a747c49c8 169 starty++;
TheDarkDurzo 8:1c8a747c49c8 170 break;
TheDarkDurzo 8:1c8a747c49c8 171 }
TheDarkDurzo 8:1c8a747c49c8 172 }
TheDarkDurzo 8:1c8a747c49c8 173 } else if(wallRight == 0) {
TheDarkDurzo 8:1c8a747c49c8 174 switch(ausr) {
TheDarkDurzo 8:1c8a747c49c8 175 case 1: {
TheDarkDurzo 8:1c8a747c49c8 176 ausr = 3;
TheDarkDurzo 8:1c8a747c49c8 177 lab[startx][starty] = ausr;
TheDarkDurzo 8:1c8a747c49c8 178 startx++;
TheDarkDurzo 8:1c8a747c49c8 179 break;
TheDarkDurzo 8:1c8a747c49c8 180 }
TheDarkDurzo 8:1c8a747c49c8 181 case 2: {
TheDarkDurzo 8:1c8a747c49c8 182 ausr = 1;
TheDarkDurzo 8:1c8a747c49c8 183 lab[startx][starty] = ausr;
TheDarkDurzo 8:1c8a747c49c8 184 starty--;
TheDarkDurzo 8:1c8a747c49c8 185 break;
TheDarkDurzo 8:1c8a747c49c8 186 }
TheDarkDurzo 8:1c8a747c49c8 187 case 3: {
TheDarkDurzo 8:1c8a747c49c8 188 ausr = 4;
TheDarkDurzo 8:1c8a747c49c8 189 lab[startx][starty] = ausr;
TheDarkDurzo 8:1c8a747c49c8 190 starty++;
TheDarkDurzo 8:1c8a747c49c8 191 break;
TheDarkDurzo 8:1c8a747c49c8 192 }
TheDarkDurzo 8:1c8a747c49c8 193 case 4: {
TheDarkDurzo 8:1c8a747c49c8 194 ausr = 2;
TheDarkDurzo 8:1c8a747c49c8 195 lab[startx][starty] = ausr;
TheDarkDurzo 8:1c8a747c49c8 196 startx--;
TheDarkDurzo 8:1c8a747c49c8 197 break;
TheDarkDurzo 8:1c8a747c49c8 198 }
TheDarkDurzo 8:1c8a747c49c8 199 }
TheDarkDurzo 8:1c8a747c49c8 200 } else {
TheDarkDurzo 8:1c8a747c49c8 201 switch(ausr) {
TheDarkDurzo 8:1c8a747c49c8 202 case 1: {
TheDarkDurzo 8:1c8a747c49c8 203 ausr = 4;
TheDarkDurzo 8:1c8a747c49c8 204 lab[startx][starty] = ausr;
TheDarkDurzo 8:1c8a747c49c8 205 starty++;
TheDarkDurzo 8:1c8a747c49c8 206 break;
TheDarkDurzo 8:1c8a747c49c8 207 }
TheDarkDurzo 8:1c8a747c49c8 208 case 2: {
TheDarkDurzo 8:1c8a747c49c8 209 ausr = 3;
TheDarkDurzo 8:1c8a747c49c8 210 lab[startx][starty] = ausr;
TheDarkDurzo 8:1c8a747c49c8 211 startx++;
TheDarkDurzo 8:1c8a747c49c8 212 break;
TheDarkDurzo 8:1c8a747c49c8 213 }
TheDarkDurzo 8:1c8a747c49c8 214 case 3: {
TheDarkDurzo 8:1c8a747c49c8 215 ausr = 2;
TheDarkDurzo 8:1c8a747c49c8 216 lab[startx][starty] = ausr;
TheDarkDurzo 8:1c8a747c49c8 217 startx--;
TheDarkDurzo 8:1c8a747c49c8 218 break;
TheDarkDurzo 8:1c8a747c49c8 219 }
TheDarkDurzo 8:1c8a747c49c8 220 case 4: {
TheDarkDurzo 8:1c8a747c49c8 221 ausr = 1;
TheDarkDurzo 8:1c8a747c49c8 222 lab[startx][starty] = ausr;
TheDarkDurzo 8:1c8a747c49c8 223 starty--;
TheDarkDurzo 8:1c8a747c49c8 224 break;
TheDarkDurzo 8:1c8a747c49c8 225 }
TheDarkDurzo 8:1c8a747c49c8 226 }
TheDarkDurzo 8:1c8a747c49c8 227 }
ruesipat 9:ab19796bf14a 228
ruesipat 5:b8b1a979b0d5 229 drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden
ruesipat 9:ab19796bf14a 230
ruesipat 5:b8b1a979b0d5 231 if(blackLine == 1) { //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier.
TheDarkDurzo 8:1c8a747c49c8 232 lab[startx][starty] = 5;
ruesipat 5:b8b1a979b0d5 233 state = 2;
ruesipat 5:b8b1a979b0d5 234 }
ruesipat 5:b8b1a979b0d5 235 break;
ruesipat 5:b8b1a979b0d5 236 }
ruesipat 9:ab19796bf14a 237
ruesipat 9:ab19796bf14a 238 case 2: { //Ziel erreicht => Wartemodus fuer 2. Lauf und Berechnung der schnellsten Weges
ruesipat 5:b8b1a979b0d5 239
ruesipat 5:b8b1a979b0d5 240 controller.setDesiredSpeedRight(0.0f);
ruesipat 5:b8b1a979b0d5 241 controller.setDesiredSpeedLeft(0.0f);
ruesipat 5:b8b1a979b0d5 242
TheDarkDurzo 8:1c8a747c49c8 243 startx = 0;
TheDarkDurzo 8:1c8a747c49c8 244 starty = 9;
ruesipat 9:ab19796bf14a 245
TheDarkDurzo 8:1c8a747c49c8 246 int num;
ruesipat 9:ab19796bf14a 247 c = 0;
ruesipat 9:ab19796bf14a 248
TheDarkDurzo 8:1c8a747c49c8 249 //Speichert die 2-Dimensionale Karte in ein 1D-Array welche nun aus anweisungen besteht
ruesipat 9:ab19796bf14a 250
TheDarkDurzo 8:1c8a747c49c8 251 while(lab[startx][starty] != 5) {
TheDarkDurzo 8:1c8a747c49c8 252 num = lab[startx][starty];
TheDarkDurzo 8:1c8a747c49c8 253 pathArr[c] = num;
TheDarkDurzo 8:1c8a747c49c8 254
TheDarkDurzo 8:1c8a747c49c8 255 switch(num) {
TheDarkDurzo 8:1c8a747c49c8 256 case 1: {
TheDarkDurzo 8:1c8a747c49c8 257 starty--;
TheDarkDurzo 8:1c8a747c49c8 258 break;
TheDarkDurzo 8:1c8a747c49c8 259 }
TheDarkDurzo 8:1c8a747c49c8 260 case 2: {
TheDarkDurzo 8:1c8a747c49c8 261 startx--;
TheDarkDurzo 8:1c8a747c49c8 262 break;
TheDarkDurzo 8:1c8a747c49c8 263 }
TheDarkDurzo 8:1c8a747c49c8 264 case 3: {
TheDarkDurzo 8:1c8a747c49c8 265 startx++;
TheDarkDurzo 8:1c8a747c49c8 266 break;
TheDarkDurzo 8:1c8a747c49c8 267 }
TheDarkDurzo 8:1c8a747c49c8 268 case 4: {
TheDarkDurzo 8:1c8a747c49c8 269 starty++;
TheDarkDurzo 8:1c8a747c49c8 270 break;
TheDarkDurzo 8:1c8a747c49c8 271 }
TheDarkDurzo 8:1c8a747c49c8 272 case 5: {
TheDarkDurzo 8:1c8a747c49c8 273
TheDarkDurzo 8:1c8a747c49c8 274 }
TheDarkDurzo 8:1c8a747c49c8 275 }//Switch End
TheDarkDurzo 8:1c8a747c49c8 276
TheDarkDurzo 8:1c8a747c49c8 277 myled =! myled; // LED is ON Heartbeat
TheDarkDurzo 8:1c8a747c49c8 278 wait(0.4f);
TheDarkDurzo 8:1c8a747c49c8 279 c++;
TheDarkDurzo 8:1c8a747c49c8 280 }//While Ende
TheDarkDurzo 8:1c8a747c49c8 281
TheDarkDurzo 8:1c8a747c49c8 282 pathArr[c]= 5; //setzt end bit
TheDarkDurzo 8:1c8a747c49c8 283
TheDarkDurzo 8:1c8a747c49c8 284
TheDarkDurzo 8:1c8a747c49c8 285 c=0;
TheDarkDurzo 8:1c8a747c49c8 286
TheDarkDurzo 8:1c8a747c49c8 287
TheDarkDurzo 8:1c8a747c49c8 288
TheDarkDurzo 8:1c8a747c49c8 289 //übersetzt den 1-Dimensionalen Array der Ausrichtung in einen Array nun Anweisungen zur Lösung darstellt
ruesipat 9:ab19796bf14a 290
TheDarkDurzo 8:1c8a747c49c8 291 while(pathArr[c]!= 5) {
TheDarkDurzo 8:1c8a747c49c8 292 if(c==0) {
TheDarkDurzo 8:1c8a747c49c8 293 if(pathArr[c]==1) {
TheDarkDurzo 8:1c8a747c49c8 294 directions[c]=1;
TheDarkDurzo 8:1c8a747c49c8 295 } else {
TheDarkDurzo 8:1c8a747c49c8 296 directions[c]=3;
TheDarkDurzo 8:1c8a747c49c8 297 }
TheDarkDurzo 8:1c8a747c49c8 298 } else if(pathArr[c-1]==pathArr[c]) {
TheDarkDurzo 8:1c8a747c49c8 299 directions[c]=1;
TheDarkDurzo 8:1c8a747c49c8 300 } else {
TheDarkDurzo 8:1c8a747c49c8 301
TheDarkDurzo 8:1c8a747c49c8 302 if(pathArr[c]==1) {
TheDarkDurzo 8:1c8a747c49c8 303 if(pathArr[c-1]==3) {
TheDarkDurzo 8:1c8a747c49c8 304 directions[c]=2;
TheDarkDurzo 8:1c8a747c49c8 305 }
TheDarkDurzo 8:1c8a747c49c8 306 if(pathArr[c-1]==2) {
TheDarkDurzo 8:1c8a747c49c8 307 directions[c]=3;
TheDarkDurzo 8:1c8a747c49c8 308 }
TheDarkDurzo 8:1c8a747c49c8 309 if(pathArr[c-1]==4) {
TheDarkDurzo 8:1c8a747c49c8 310 directions[c-1]=4;
TheDarkDurzo 8:1c8a747c49c8 311 }
TheDarkDurzo 8:1c8a747c49c8 312 } else if(pathArr[c]==2) {
TheDarkDurzo 8:1c8a747c49c8 313 if(pathArr[c-1]==1) {
TheDarkDurzo 8:1c8a747c49c8 314 directions[c]=2;
TheDarkDurzo 8:1c8a747c49c8 315 }
TheDarkDurzo 8:1c8a747c49c8 316 if(pathArr[c-1]==3) {
TheDarkDurzo 8:1c8a747c49c8 317 directions[c]=4;
TheDarkDurzo 8:1c8a747c49c8 318 }
TheDarkDurzo 8:1c8a747c49c8 319 if(pathArr[c-1]==4) {
TheDarkDurzo 8:1c8a747c49c8 320 directions[c]=3;
TheDarkDurzo 8:1c8a747c49c8 321 }
TheDarkDurzo 8:1c8a747c49c8 322 } else if(pathArr[c]==3) {
TheDarkDurzo 8:1c8a747c49c8 323 if(pathArr[c-1]==1) {
TheDarkDurzo 8:1c8a747c49c8 324 directions[c]=3;
TheDarkDurzo 8:1c8a747c49c8 325 }
TheDarkDurzo 8:1c8a747c49c8 326 if(pathArr[c-1]==2) {
TheDarkDurzo 8:1c8a747c49c8 327 directions[c]=4;
TheDarkDurzo 8:1c8a747c49c8 328 }
TheDarkDurzo 8:1c8a747c49c8 329 if(pathArr[c-1]==4) {
TheDarkDurzo 8:1c8a747c49c8 330 directions[c]=2;
TheDarkDurzo 8:1c8a747c49c8 331 }
TheDarkDurzo 8:1c8a747c49c8 332 } else if(pathArr[c]==4) {
TheDarkDurzo 8:1c8a747c49c8 333 if(pathArr[c-1]==1) {
TheDarkDurzo 8:1c8a747c49c8 334 directions[c]=4;
TheDarkDurzo 8:1c8a747c49c8 335 }
TheDarkDurzo 8:1c8a747c49c8 336 if(pathArr[c-1]==2) {
TheDarkDurzo 8:1c8a747c49c8 337 directions[c]=2;
TheDarkDurzo 8:1c8a747c49c8 338 }
TheDarkDurzo 8:1c8a747c49c8 339 if(pathArr[c-1]==3) {
TheDarkDurzo 8:1c8a747c49c8 340 directions[c]=3;
TheDarkDurzo 8:1c8a747c49c8 341 }
TheDarkDurzo 8:1c8a747c49c8 342 }
TheDarkDurzo 8:1c8a747c49c8 343 }
TheDarkDurzo 8:1c8a747c49c8 344 c++;
TheDarkDurzo 8:1c8a747c49c8 345 myled =! myled; // LED is ON Heartbeat
TheDarkDurzo 8:1c8a747c49c8 346 wait(0.1f);
TheDarkDurzo 8:1c8a747c49c8 347 }
ruesipat 9:ab19796bf14a 348 c = 0;
TheDarkDurzo 8:1c8a747c49c8 349
ruesipat 5:b8b1a979b0d5 350 while (button) {
ruesipat 5:b8b1a979b0d5 351 wait(1.0f);
ruesipat 5:b8b1a979b0d5 352 }
ruesipat 5:b8b1a979b0d5 353 blackLine = 0;
ruesipat 9:ab19796bf14a 354 firstMove = 1;
ruesipat 5:b8b1a979b0d5 355 state = 3;
ruesipat 9:ab19796bf14a 356 myled = 0; // LED is ON Heartbeat
ruesipat 9:ab19796bf14a 357 c = 0;
ruesipat 5:b8b1a979b0d5 358 break;
ruesipat 5:b8b1a979b0d5 359 }
ruesipat 5:b8b1a979b0d5 360 case 3: {//2. Lauf Fahrzyklus
ruesipat 9:ab19796bf14a 361
ruesipat 9:ab19796bf14a 362 //uebergibt Anweisung an turn
TheDarkDurzo 8:1c8a747c49c8 363
ruesipat 9:ab19796bf14a 364 modeStart=1;
ruesipat 9:ab19796bf14a 365 path = directions[c];
ruesipat 9:ab19796bf14a 366 pathNext = directions[c+1];
TheDarkDurzo 8:1c8a747c49c8 367
ruesipat 9:ab19796bf14a 368
ruesipat 9:ab19796bf14a 369 turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
TheDarkDurzo 8:1c8a747c49c8 370
ruesipat 9:ab19796bf14a 371
ruesipat 9:ab19796bf14a 372 /**************************/
ruesipat 9:ab19796bf14a 373 if(firstMove == 1){
ruesipat 9:ab19796bf14a 374 dontStop = 1;
ruesipat 9:ab19796bf14a 375 firstMove = 0;
ruesipat 9:ab19796bf14a 376 }
ruesipat 9:ab19796bf14a 377 /***********************/
ruesipat 9:ab19796bf14a 378
ruesipat 9:ab19796bf14a 379
ruesipat 9:ab19796bf14a 380
ruesipat 5:b8b1a979b0d5 381 drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden
TheDarkDurzo 8:1c8a747c49c8 382 c++;
ruesipat 9:ab19796bf14a 383
ruesipat 5:b8b1a979b0d5 384 if(blackLine == 1) { //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier.
ruesipat 5:b8b1a979b0d5 385 state = 4;
ruesipat 5:b8b1a979b0d5 386 }
ruesipat 5:b8b1a979b0d5 387 break;
ruesipat 5:b8b1a979b0d5 388 }
ruesipat 5:b8b1a979b0d5 389 case 4: { //Ziel erreicht und Wartemodus fuer 2. Lauf
ruesipat 5:b8b1a979b0d5 390 controller.setDesiredSpeedRight(0.0f);
ruesipat 5:b8b1a979b0d5 391 controller.setDesiredSpeedLeft(0.0f);
ruesipat 5:b8b1a979b0d5 392
ruesipat 5:b8b1a979b0d5 393 while (button) {
ruesipat 5:b8b1a979b0d5 394 wait(1.0f);
ruesipat 5:b8b1a979b0d5 395 }
ruesipat 5:b8b1a979b0d5 396 state = 5;
ruesipat 5:b8b1a979b0d5 397 break;
ruesipat 5:b8b1a979b0d5 398 }
ruesipat 5:b8b1a979b0d5 399 default: {
ruesipat 5:b8b1a979b0d5 400 state = 0;
ruesipat 5:b8b1a979b0d5 401 break;
ruesipat 5:b8b1a979b0d5 402 }
ruesipat 0:a9fe4ef404bf 403 }
ruesipat 5:b8b1a979b0d5 404 myled =! myled; // LED is ON Heartbeat
ruesipat 5:b8b1a979b0d5 405 wait(0.1f);
ruesipat 0:a9fe4ef404bf 406 }
ruesipat 0:a9fe4ef404bf 407 }