---
Dependencies: mbed
Fork of MicroMouse_MASTER_FIVE by
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?
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 | 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 | } |