Pathfinding nach rechts funktioniert noch nicht...der rest schon
Dependencies: mbed
Fork of MicroMouse_MASTER_THREE by
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?
User | Revision | Line number | New 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 | } |