PES2_R2D2.0 / Mbed 2 deprecated MicroMouse_MASTER_FIVE

ENDLÖSUNG:)

Dependencies:   mbed

Fork of MicroMouse_MASTER_FOUR by PES2_R2D2.0

main.cpp@10:e8110fb94686, 2018-05-22 (annotated)

Committer:
ruesipat
Date:
Tue May 22 14:20:20 2018 +0000
Revision:
10:e8110fb94686
Parent:
9:ab19796bf14a 
s

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 10:e8110fb94686 49 //printf("--------\n\rREADY\n\r--------\n\r");
ruesipat 5:b8b1a979b0d5 50 enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
ruesipat 5:b8b1a979b0d5 51 enable = 1;
ruesipat 5:b8b1a979b0d5 52
ruesipat 4:e3f388933954 53 int wallRight = 0;
ruesipat 4:e3f388933954 54 int wallFront = 0;
ruesipat 5:b8b1a979b0d5 55 int wallLeft = 0;
ruesipat 4:e3f388933954 56 int blackLine = 0;
ruesipat 7:5ef09519a6e9 57 int dontStop = 0;
ruesipat 9:ab19796bf14a 58 int lab[5][10];
ruesipat 9:ab19796bf14a 59 int path = 0;
ruesipat 9:ab19796bf14a 60 int pathNext = 0;
ruesipat 10:e8110fb94686 61 int pathArr[51];
ruesipat 10:e8110fb94686 62 int directions[51];
ruesipat 9:ab19796bf14a 63 int modeStart = 0;
ruesipat 9:ab19796bf14a 64 int startx = 0;
ruesipat 10:e8110fb94686 65 int starty = 9;
ruesipat 9:ab19796bf14a 66 int ausr = 1;
ruesipat 9:ab19796bf14a 67 int c = 0;
ruesipat 9:ab19796bf14a 68 int firstMove = 0;
ruesipat 10:e8110fb94686 69
ruesipat 7:5ef09519a6e9 70 //Funktionsdeklarationen
ruesipat 10:e8110fb94686 71
ruesipat 5:b8b1a979b0d5 72 CheckWalls checkWalls(irSensor0, irSensor1, irSensor2, wallRight, wallFront, wallLeft); //Ermittlung wo freie Wege(0) bzw welche Waende vorhanden (1)
TheDarkDurzo 8:1c8a747c49c8 73 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 74 KontrastSensor kontrastSensor(kontrast, blackLine);
ruesipat 10:e8110fb94686 75 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 76
ruesipat 2:592f01278db4 77
ruesipat 1:d9e840c48b1e 78 while(1) { // Wiederholungsschleife
ruesipat 10:e8110fb94686 79
ruesipat 5:b8b1a979b0d5 80 switch(state) { //Wartemodus 1. Lauf
ruesipat 5:b8b1a979b0d5 81 case 0: {
ruesipat 5:b8b1a979b0d5 82 while (button) {
ruesipat 5:b8b1a979b0d5 83 wait(1.0f); //Die Ruhe vor dem Sturm...
ruesipat 5:b8b1a979b0d5 84 }
ruesipat 9:ab19796bf14a 85 firstMove = 1;
ruesipat 5:b8b1a979b0d5 86 state = 1;
ruesipat 10:e8110fb94686 87
ruesipat 5:b8b1a979b0d5 88 break;
ruesipat 5:b8b1a979b0d5 89 }
ruesipat 5:b8b1a979b0d5 90 case 1: {// 1.Lauf Fahrzyklus und Aufzeichungszyklus
ruesipat 10:e8110fb94686 91
ruesipat 5:b8b1a979b0d5 92 checkWalls.check(); //prueft wo Waende vorhanden sind
ruesipat 10:e8110fb94686 93
ruesipat 5:b8b1a979b0d5 94
ruesipat 10:e8110fb94686 95
TheDarkDurzo 8:1c8a747c49c8 96 turn.turning();
ruesipat 10:e8110fb94686 97
ruesipat 10:e8110fb94686 98 if(firstMove == 1) {
ruesipat 9:ab19796bf14a 99 dontStop = 1;
ruesipat 9:ab19796bf14a 100 firstMove = 0;
ruesipat 10:e8110fb94686 101
ruesipat 9:ab19796bf14a 102 /* **************************** */
ruesipat 9:ab19796bf14a 103 /* Anpassung 16.02.18 */
ruesipat 10:e8110fb94686 104 //if (wallFront == 0) {
ruesipat 10:e8110fb94686 105 // ausr = 1;
ruesipat 10:e8110fb94686 106 // lab[startx][starty] = ausr;
ruesipat 10:e8110fb94686 107 // starty--;
ruesipat 10:e8110fb94686 108 //} else if (wallRight == 20) {
ruesipat 10:e8110fb94686 109 // ausr = 3; // Ausrichtung zu kontrollieren!!!!
ruesipat 10:e8110fb94686 110 // lab[startx][starty] = ausr;
ruesipat 10:e8110fb94686 111 // startx++;
ruesipat 10:e8110fb94686 112 //}
ruesipat 10:e8110fb94686 113 /***********************/
ruesipat 9:ab19796bf14a 114 }
ruesipat 10:e8110fb94686 115
ruesipat 9:ab19796bf14a 116 //merkt sich wie sich die Ausrichtung des Fahrzeugs im Labyrinth veraendert und speichert diese in ein Array
ruesipat 10:e8110fb94686 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 10:e8110fb94686 228
ruesipat 5:b8b1a979b0d5 229 drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden
ruesipat 10:e8110fb94686 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 10:e8110fb94686 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 10:e8110fb94686 245
TheDarkDurzo 8:1c8a747c49c8 246 int num;
ruesipat 9:ab19796bf14a 247 c = 0;
ruesipat 10:e8110fb94686 248
TheDarkDurzo 8:1c8a747c49c8 249 //Speichert die 2-Dimensionale Karte in ein 1D-Array welche nun aus anweisungen besteht
ruesipat 10:e8110fb94686 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
ruesipat 10:e8110fb94686 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
ruesipat 10:e8110fb94686 281
TheDarkDurzo 8:1c8a747c49c8 282 pathArr[c]= 5; //setzt end bit
ruesipat 10:e8110fb94686 283
ruesipat 10:e8110fb94686 284
TheDarkDurzo 8:1c8a747c49c8 285 c=0;
TheDarkDurzo 8:1c8a747c49c8 286
ruesipat 10:e8110fb94686 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 10:e8110fb94686 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 {
ruesipat 10:e8110fb94686 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 10:e8110fb94686 350
ruesipat 10:e8110fb94686 351 for(int p=0; p<20; p++) {
ruesipat 10:e8110fb94686 352 //printf("Anw: %d\r\n", directions[p]);
ruesipat 10:e8110fb94686 353 }
ruesipat 10:e8110fb94686 354
ruesipat 10:e8110fb94686 355 for(int p=0; p<9; p++) {
ruesipat 10:e8110fb94686 356 for(int q=0; q<4; q++) {
ruesipat 10:e8110fb94686 357 //printf(" %d ", lab[q][p]);
ruesipat 10:e8110fb94686 358 }
ruesipat 10:e8110fb94686 359 //printf("\r\n");
ruesipat 10:e8110fb94686 360 }
ruesipat 10:e8110fb94686 361
ruesipat 10:e8110fb94686 362
ruesipat 10:e8110fb94686 363
ruesipat 10:e8110fb94686 364
ruesipat 5:b8b1a979b0d5 365 while (button) {
ruesipat 5:b8b1a979b0d5 366 wait(1.0f);
ruesipat 5:b8b1a979b0d5 367 }
ruesipat 5:b8b1a979b0d5 368 blackLine = 0;
ruesipat 9:ab19796bf14a 369 firstMove = 1;
ruesipat 5:b8b1a979b0d5 370 state = 3;
ruesipat 9:ab19796bf14a 371 myled = 0; // LED is ON Heartbeat
ruesipat 9:ab19796bf14a 372 c = 0;
ruesipat 5:b8b1a979b0d5 373 break;
ruesipat 5:b8b1a979b0d5 374 }
ruesipat 5:b8b1a979b0d5 375 case 3: {//2. Lauf Fahrzyklus
ruesipat 10:e8110fb94686 376
ruesipat 9:ab19796bf14a 377 //uebergibt Anweisung an turn
ruesipat 10:e8110fb94686 378
ruesipat 9:ab19796bf14a 379 modeStart=1;
ruesipat 9:ab19796bf14a 380 path = directions[c];
ruesipat 10:e8110fb94686 381 pathNext = directions[c+1];
ruesipat 10:e8110fb94686 382
ruesipat 10:e8110fb94686 383
ruesipat 9:ab19796bf14a 384 turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
ruesipat 10:e8110fb94686 385
ruesipat 10:e8110fb94686 386
ruesipat 9:ab19796bf14a 387 /**************************/
ruesipat 10:e8110fb94686 388 if(firstMove == 1) {
ruesipat 9:ab19796bf14a 389 dontStop = 1;
ruesipat 9:ab19796bf14a 390 firstMove = 0;
ruesipat 9:ab19796bf14a 391 }
ruesipat 9:ab19796bf14a 392 /***********************/
ruesipat 10:e8110fb94686 393
ruesipat 10:e8110fb94686 394
ruesipat 10:e8110fb94686 395
ruesipat 5:b8b1a979b0d5 396 drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden
TheDarkDurzo 8:1c8a747c49c8 397 c++;
ruesipat 10:e8110fb94686 398
ruesipat 10:e8110fb94686 399
ruesipat 10:e8110fb94686 400
ruesipat 5:b8b1a979b0d5 401 if(blackLine == 1) { //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier.
ruesipat 5:b8b1a979b0d5 402 state = 4;
ruesipat 5:b8b1a979b0d5 403 }
ruesipat 5:b8b1a979b0d5 404 break;
ruesipat 5:b8b1a979b0d5 405 }
ruesipat 5:b8b1a979b0d5 406 case 4: { //Ziel erreicht und Wartemodus fuer 2. Lauf
ruesipat 5:b8b1a979b0d5 407 controller.setDesiredSpeedRight(0.0f);
ruesipat 5:b8b1a979b0d5 408 controller.setDesiredSpeedLeft(0.0f);
ruesipat 5:b8b1a979b0d5 409
ruesipat 5:b8b1a979b0d5 410 while (button) {
ruesipat 5:b8b1a979b0d5 411 wait(1.0f);
ruesipat 5:b8b1a979b0d5 412 }
ruesipat 5:b8b1a979b0d5 413 state = 5;
ruesipat 5:b8b1a979b0d5 414 break;
ruesipat 5:b8b1a979b0d5 415 }
ruesipat 5:b8b1a979b0d5 416 default: {
ruesipat 5:b8b1a979b0d5 417 state = 0;
ruesipat 5:b8b1a979b0d5 418 break;
ruesipat 5:b8b1a979b0d5 419 }
ruesipat 0:a9fe4ef404bf 420 }
ruesipat 5:b8b1a979b0d5 421 myled =! myled; // LED is ON Heartbeat
ruesipat 5:b8b1a979b0d5 422 wait(0.1f);
ruesipat 0:a9fe4ef404bf 423 }
ruesipat 0:a9fe4ef404bf 424 }