Pathfinding nach rechts funktioniert noch nicht...der rest schon
Dependencies: mbed
Fork of MicroMouse_MASTER_THREE by
main.cpp@8:1c8a747c49c8, 2018-05-16 (annotated)
- Committer:
- TheDarkDurzo
- Date:
- Wed May 16 12:15:23 2018 +0000
- Revision:
- 8:1c8a747c49c8
- Parent:
- 7:5ef09519a6e9
- Child:
- 9:ab19796bf14a
mit pathfinding
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 | 1:d9e840c48b1e | 14 | //Initialisierung LEDs Blinker/Surri/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 | DigitalOut blinker0(PA_4); //Blinker links |
ruesipat | 1:d9e840c48b1e | 18 | DigitalOut blinker1(PC_1); //Blinker rechts |
ruesipat | 1:d9e840c48b1e | 19 | DigitalOut surri(PC_0); // |
ruesipat | 1:d9e840c48b1e | 20 | |
ruesipat | 1:d9e840c48b1e | 21 | //Initialisierung LEDs Nightrider |
ruesipat | 1:d9e840c48b1e | 22 | PwmOut led0(PB_0); //von links nach rechts |
ruesipat | 1:d9e840c48b1e | 23 | PwmOut led1(PB_8); |
ruesipat | 1:d9e840c48b1e | 24 | PwmOut led2(PB_3); |
ruesipat | 1:d9e840c48b1e | 25 | PwmOut led3(PB_5); |
ruesipat | 1:d9e840c48b1e | 26 | PwmOut led4(PB_4); |
ruesipat | 1:d9e840c48b1e | 27 | PwmOut led5(PB_10); |
ruesipat | 1:d9e840c48b1e | 28 | PwmOut led6(PB_9); |
ruesipat | 1:d9e840c48b1e | 29 | |
ruesipat | 1:d9e840c48b1e | 30 | //Initialisierung IR-Sensoren |
ruesipat | 1:d9e840c48b1e | 31 | AnalogIn distance0(PC_2); //Kreieren der Eingangsobjekte |
ruesipat | 1:d9e840c48b1e | 32 | AnalogIn distance1(PC_3); |
ruesipat | 1:d9e840c48b1e | 33 | AnalogIn distance2(PC_5); |
ruesipat | 1:d9e840c48b1e | 34 | AnalogIn distance3(PB_1); |
ruesipat | 1:d9e840c48b1e | 35 | IRSensor irSensor0(distance0); //rechts |
ruesipat | 1:d9e840c48b1e | 36 | IRSensor irSensor1(distance1); //vorne |
ruesipat | 1:d9e840c48b1e | 37 | IRSensor irSensor2(distance2); //links-vorne |
ruesipat | 5:b8b1a979b0d5 | 38 | IRSensor irSensor3(distance3); //links-hinten |
ruesipat | 1:d9e840c48b1e | 39 | |
ruesipat | 1:d9e840c48b1e | 40 | //Initialisierung Kontrastsensor |
ruesipat | 1:d9e840c48b1e | 41 | AnalogIn kontrast(PC_0); |
ruesipat | 1:d9e840c48b1e | 42 | |
ruesipat | 1:d9e840c48b1e | 43 | //Initialisierung Motor und Encoder |
ruesipat | 0:a9fe4ef404bf | 44 | DigitalOut enableMotorDriver(PB_2); |
ruesipat | 1:d9e840c48b1e | 45 | PwmOut pwmLeft(PA_8); //Motor links |
ruesipat | 1:d9e840c48b1e | 46 | PwmOut pwmRight(PA_9); //Motor rechts |
ruesipat | 1:d9e840c48b1e | 47 | EncoderCounter counterLeft(PB_6, PB_7); //Encoder für Motor links |
ruesipat | 1:d9e840c48b1e | 48 | EncoderCounter counterRight(PA_6, PC_7);//Encoder für Motor rechts |
ruesipat | 0:a9fe4ef404bf | 49 | |
ruesipat | 0:a9fe4ef404bf | 50 | DigitalOut enable(PC_1); |
ruesipat | 0:a9fe4ef404bf | 51 | |
ruesipat | 1:d9e840c48b1e | 52 | Controller controller(pwmLeft, pwmRight, counterLeft, counterRight); |
ruesipat | 0:a9fe4ef404bf | 53 | |
ruesipat | 1:d9e840c48b1e | 54 | int state = 0; |
TheDarkDurzo | 8:1c8a747c49c8 | 55 | int mode = 1; |
TheDarkDurzo | 8:1c8a747c49c8 | 56 | |
ruesipat | 1:d9e840c48b1e | 57 | |
ruesipat | 1:d9e840c48b1e | 58 | int main() |
ruesipat | 1:d9e840c48b1e | 59 | { |
TheDarkDurzo | 8:1c8a747c49c8 | 60 | printf("Start main\r\n"); |
TheDarkDurzo | 8:1c8a747c49c8 | 61 | |
TheDarkDurzo | 8:1c8a747c49c8 | 62 | |
TheDarkDurzo | 8:1c8a747c49c8 | 63 | |
TheDarkDurzo | 8:1c8a747c49c8 | 64 | |
TheDarkDurzo | 8:1c8a747c49c8 | 65 | int lab[5][10]; |
TheDarkDurzo | 8:1c8a747c49c8 | 66 | int path = 0; |
TheDarkDurzo | 8:1c8a747c49c8 | 67 | int pathArr[50]; |
TheDarkDurzo | 8:1c8a747c49c8 | 68 | int directions[50]; |
TheDarkDurzo | 8:1c8a747c49c8 | 69 | int modeStart=0; |
TheDarkDurzo | 8:1c8a747c49c8 | 70 | int startx = 0; |
TheDarkDurzo | 8:1c8a747c49c8 | 71 | int starty = 10; |
TheDarkDurzo | 8:1c8a747c49c8 | 72 | int ausr = 1; |
TheDarkDurzo | 8:1c8a747c49c8 | 73 | int c = 0; |
ruesipat | 5:b8b1a979b0d5 | 74 | enableMotorDriver = 1; // Schaltet den Leistungstreiber ein |
ruesipat | 5:b8b1a979b0d5 | 75 | enable = 1; |
ruesipat | 5:b8b1a979b0d5 | 76 | |
ruesipat | 4:e3f388933954 | 77 | int wallRight = 0; |
ruesipat | 4:e3f388933954 | 78 | int wallFront = 0; |
ruesipat | 5:b8b1a979b0d5 | 79 | int wallLeft = 0; |
ruesipat | 4:e3f388933954 | 80 | int blackLine = 0; |
ruesipat | 7:5ef09519a6e9 | 81 | int dontStop = 0; |
ruesipat | 7:5ef09519a6e9 | 82 | //int way = 220; |
TheDarkDurzo | 8:1c8a747c49c8 | 83 | |
ruesipat | 5:b8b1a979b0d5 | 84 | // int way[5][10][4] = {{0},{0},{0}}; // Abspeicherung wird in roboterkoordinaten gespeichert vierstellige Zahl => 1.Zahl gefahrene Richtung, 2.Zahl Wand rechts, 3.Zahl Wand vorne, 4.Zahl Wand links |
TheDarkDurzo | 8:1c8a747c49c8 | 85 | |
ruesipat | 7:5ef09519a6e9 | 86 | //Funktionsdeklarationen |
ruesipat | 5:b8b1a979b0d5 | 87 | CheckWalls checkWalls(irSensor0, irSensor1, irSensor2, wallRight, wallFront, wallLeft); //Ermittlung wo freie Wege(0) bzw welche Waende vorhanden (1) |
TheDarkDurzo | 8:1c8a747c49c8 | 88 | 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 | 89 | KontrastSensor kontrastSensor(kontrast, blackLine); |
TheDarkDurzo | 8:1c8a747c49c8 | 90 | Drive drive(kontrastSensor, counterLeft, counterRight, controller, irSensor0, irSensor1, irSensor2, irSensor3, dontStop); //20cm nach vorne...//Ablauf ist jeweils immer Feldweise...Waende checken, allenfalls drehen (Ausrichten), waehrend der Fahrt nur Ausrichtung der Geradeausfahrt... |
ruesipat | 0:a9fe4ef404bf | 91 | |
ruesipat | 2:592f01278db4 | 92 | |
ruesipat | 1:d9e840c48b1e | 93 | while(1) { // Wiederholungsschleife |
ruesipat | 0:a9fe4ef404bf | 94 | |
ruesipat | 5:b8b1a979b0d5 | 95 | |
ruesipat | 5:b8b1a979b0d5 | 96 | switch(state) { //Wartemodus 1. Lauf |
ruesipat | 5:b8b1a979b0d5 | 97 | case 0: { |
ruesipat | 5:b8b1a979b0d5 | 98 | |
ruesipat | 5:b8b1a979b0d5 | 99 | |
ruesipat | 5:b8b1a979b0d5 | 100 | while (button) { |
ruesipat | 5:b8b1a979b0d5 | 101 | //printf("WARTE AUF 1.START..."); |
ruesipat | 7:5ef09519a6e9 | 102 | //controller.setDesiredSpeedRight(0.5f); |
ruesipat | 7:5ef09519a6e9 | 103 | //controller.setDesiredSpeedLeft(-0.5f); |
ruesipat | 5:b8b1a979b0d5 | 104 | wait(1.0f); //Die Ruhe vor dem Sturm... |
ruesipat | 5:b8b1a979b0d5 | 105 | } |
ruesipat | 5:b8b1a979b0d5 | 106 | |
TheDarkDurzo | 8:1c8a747c49c8 | 107 | //if(modeStart == 1) { |
TheDarkDurzo | 8:1c8a747c49c8 | 108 | // state = 1; |
TheDarkDurzo | 8:1c8a747c49c8 | 109 | //} |
TheDarkDurzo | 8:1c8a747c49c8 | 110 | |
ruesipat | 5:b8b1a979b0d5 | 111 | state = 1; |
ruesipat | 5:b8b1a979b0d5 | 112 | break; |
ruesipat | 5:b8b1a979b0d5 | 113 | } |
ruesipat | 5:b8b1a979b0d5 | 114 | case 1: {// 1.Lauf Fahrzyklus und Aufzeichungszyklus |
ruesipat | 5:b8b1a979b0d5 | 115 | |
ruesipat | 7:5ef09519a6e9 | 116 | |
ruesipat | 5:b8b1a979b0d5 | 117 | checkWalls.check(); //prueft wo Waende vorhanden sind |
ruesipat | 5:b8b1a979b0d5 | 118 | |
TheDarkDurzo | 8:1c8a747c49c8 | 119 | turn.turning(); |
TheDarkDurzo | 8:1c8a747c49c8 | 120 | |
TheDarkDurzo | 8:1c8a747c49c8 | 121 | //merkt sich wie sich die Ausrichtung des Fahrzeugs im Labyrinth verändert und speichert diese in ein Array |
TheDarkDurzo | 8:1c8a747c49c8 | 122 | if(wallLeft == 0) { |
TheDarkDurzo | 8:1c8a747c49c8 | 123 | switch(ausr) { |
TheDarkDurzo | 8:1c8a747c49c8 | 124 | case 1: { |
TheDarkDurzo | 8:1c8a747c49c8 | 125 | ausr = 2; |
TheDarkDurzo | 8:1c8a747c49c8 | 126 | lab[startx][starty] = ausr; |
TheDarkDurzo | 8:1c8a747c49c8 | 127 | startx--; |
TheDarkDurzo | 8:1c8a747c49c8 | 128 | break; |
TheDarkDurzo | 8:1c8a747c49c8 | 129 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 130 | case 2: { |
TheDarkDurzo | 8:1c8a747c49c8 | 131 | //lab[startx][starty] =2; |
TheDarkDurzo | 8:1c8a747c49c8 | 132 | ausr = 4; |
TheDarkDurzo | 8:1c8a747c49c8 | 133 | lab[startx][starty] = ausr; |
TheDarkDurzo | 8:1c8a747c49c8 | 134 | starty++; |
TheDarkDurzo | 8:1c8a747c49c8 | 135 | break; |
TheDarkDurzo | 8:1c8a747c49c8 | 136 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 137 | case 3: { |
TheDarkDurzo | 8:1c8a747c49c8 | 138 | //lab[startx][starty]=2; |
TheDarkDurzo | 8:1c8a747c49c8 | 139 | ausr = 1; |
TheDarkDurzo | 8:1c8a747c49c8 | 140 | lab[startx][starty] = ausr; |
TheDarkDurzo | 8:1c8a747c49c8 | 141 | starty--; |
TheDarkDurzo | 8:1c8a747c49c8 | 142 | break; |
TheDarkDurzo | 8:1c8a747c49c8 | 143 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 144 | case 4: { |
TheDarkDurzo | 8:1c8a747c49c8 | 145 | //lab[startx][starty]=2; |
TheDarkDurzo | 8:1c8a747c49c8 | 146 | ausr = 3; |
TheDarkDurzo | 8:1c8a747c49c8 | 147 | lab[startx][starty] = ausr; |
TheDarkDurzo | 8:1c8a747c49c8 | 148 | startx++; |
TheDarkDurzo | 8:1c8a747c49c8 | 149 | break; |
TheDarkDurzo | 8:1c8a747c49c8 | 150 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 151 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 152 | |
TheDarkDurzo | 8:1c8a747c49c8 | 153 | } else if(wallFront == 0) { |
TheDarkDurzo | 8:1c8a747c49c8 | 154 | switch(ausr) { |
TheDarkDurzo | 8:1c8a747c49c8 | 155 | case 1: { |
TheDarkDurzo | 8:1c8a747c49c8 | 156 | //lab[startx][starty]=1; |
TheDarkDurzo | 8:1c8a747c49c8 | 157 | ausr = 1; |
TheDarkDurzo | 8:1c8a747c49c8 | 158 | lab[startx][starty] = ausr; |
TheDarkDurzo | 8:1c8a747c49c8 | 159 | starty--; |
TheDarkDurzo | 8:1c8a747c49c8 | 160 | break; |
TheDarkDurzo | 8:1c8a747c49c8 | 161 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 162 | case 2: { |
TheDarkDurzo | 8:1c8a747c49c8 | 163 | //lab[startx][starty]=1; |
TheDarkDurzo | 8:1c8a747c49c8 | 164 | ausr = 2; |
TheDarkDurzo | 8:1c8a747c49c8 | 165 | lab[startx][starty] = ausr; |
TheDarkDurzo | 8:1c8a747c49c8 | 166 | startx--; |
TheDarkDurzo | 8:1c8a747c49c8 | 167 | break; |
TheDarkDurzo | 8:1c8a747c49c8 | 168 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 169 | case 3: { |
TheDarkDurzo | 8:1c8a747c49c8 | 170 | //lab[startx][starty]=1; |
TheDarkDurzo | 8:1c8a747c49c8 | 171 | ausr = 3; |
TheDarkDurzo | 8:1c8a747c49c8 | 172 | lab[startx][starty] = ausr; |
TheDarkDurzo | 8:1c8a747c49c8 | 173 | startx++; |
TheDarkDurzo | 8:1c8a747c49c8 | 174 | break; |
TheDarkDurzo | 8:1c8a747c49c8 | 175 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 176 | case 4: { |
TheDarkDurzo | 8:1c8a747c49c8 | 177 | //lab[startx][starty]=1; |
TheDarkDurzo | 8:1c8a747c49c8 | 178 | ausr = 4; |
TheDarkDurzo | 8:1c8a747c49c8 | 179 | lab[startx][starty] = ausr; |
TheDarkDurzo | 8:1c8a747c49c8 | 180 | starty++; |
TheDarkDurzo | 8:1c8a747c49c8 | 181 | break; |
TheDarkDurzo | 8:1c8a747c49c8 | 182 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 183 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 184 | } else if(wallRight == 0) { |
TheDarkDurzo | 8:1c8a747c49c8 | 185 | switch(ausr) { |
TheDarkDurzo | 8:1c8a747c49c8 | 186 | case 1: { |
TheDarkDurzo | 8:1c8a747c49c8 | 187 | //lab[startx][starty]=3; |
TheDarkDurzo | 8:1c8a747c49c8 | 188 | ausr = 3; |
TheDarkDurzo | 8:1c8a747c49c8 | 189 | lab[startx][starty] = ausr; |
TheDarkDurzo | 8:1c8a747c49c8 | 190 | startx++; |
TheDarkDurzo | 8:1c8a747c49c8 | 191 | break; |
TheDarkDurzo | 8:1c8a747c49c8 | 192 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 193 | case 2: { |
TheDarkDurzo | 8:1c8a747c49c8 | 194 | //lab[startx][starty]=3; |
TheDarkDurzo | 8:1c8a747c49c8 | 195 | ausr = 1; |
TheDarkDurzo | 8:1c8a747c49c8 | 196 | lab[startx][starty] = ausr; |
TheDarkDurzo | 8:1c8a747c49c8 | 197 | starty--; |
TheDarkDurzo | 8:1c8a747c49c8 | 198 | break; |
TheDarkDurzo | 8:1c8a747c49c8 | 199 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 200 | case 3: { |
TheDarkDurzo | 8:1c8a747c49c8 | 201 | //lab[startx][starty]=3; |
TheDarkDurzo | 8:1c8a747c49c8 | 202 | ausr = 4; |
TheDarkDurzo | 8:1c8a747c49c8 | 203 | lab[startx][starty] = ausr; |
TheDarkDurzo | 8:1c8a747c49c8 | 204 | starty++; |
TheDarkDurzo | 8:1c8a747c49c8 | 205 | break; |
TheDarkDurzo | 8:1c8a747c49c8 | 206 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 207 | case 4: { |
TheDarkDurzo | 8:1c8a747c49c8 | 208 | //lab[startx][starty]=3; |
TheDarkDurzo | 8:1c8a747c49c8 | 209 | ausr = 2; |
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 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 215 | } else { |
TheDarkDurzo | 8:1c8a747c49c8 | 216 | switch(ausr) { |
TheDarkDurzo | 8:1c8a747c49c8 | 217 | case 1: { |
TheDarkDurzo | 8:1c8a747c49c8 | 218 | //lab[startx][starty]=4; |
TheDarkDurzo | 8:1c8a747c49c8 | 219 | ausr = 4; |
TheDarkDurzo | 8:1c8a747c49c8 | 220 | lab[startx][starty] = ausr; |
TheDarkDurzo | 8:1c8a747c49c8 | 221 | starty++; |
TheDarkDurzo | 8:1c8a747c49c8 | 222 | break; |
TheDarkDurzo | 8:1c8a747c49c8 | 223 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 224 | case 2: { |
TheDarkDurzo | 8:1c8a747c49c8 | 225 | //lab[startx][starty]=4; |
TheDarkDurzo | 8:1c8a747c49c8 | 226 | ausr = 3; |
TheDarkDurzo | 8:1c8a747c49c8 | 227 | lab[startx][starty] = ausr; |
TheDarkDurzo | 8:1c8a747c49c8 | 228 | startx++; |
TheDarkDurzo | 8:1c8a747c49c8 | 229 | break; |
TheDarkDurzo | 8:1c8a747c49c8 | 230 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 231 | case 3: { |
TheDarkDurzo | 8:1c8a747c49c8 | 232 | //lab[startx][starty]=4; |
TheDarkDurzo | 8:1c8a747c49c8 | 233 | ausr = 2; |
TheDarkDurzo | 8:1c8a747c49c8 | 234 | lab[startx][starty] = ausr; |
TheDarkDurzo | 8:1c8a747c49c8 | 235 | startx--; |
TheDarkDurzo | 8:1c8a747c49c8 | 236 | break; |
TheDarkDurzo | 8:1c8a747c49c8 | 237 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 238 | case 4: { |
TheDarkDurzo | 8:1c8a747c49c8 | 239 | //lab[startx][starty]=4; |
TheDarkDurzo | 8:1c8a747c49c8 | 240 | ausr = 1; |
TheDarkDurzo | 8:1c8a747c49c8 | 241 | lab[startx][starty] = ausr; |
TheDarkDurzo | 8:1c8a747c49c8 | 242 | starty--; |
TheDarkDurzo | 8:1c8a747c49c8 | 243 | break; |
TheDarkDurzo | 8:1c8a747c49c8 | 244 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 245 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 246 | } |
ruesipat | 5:b8b1a979b0d5 | 247 | |
ruesipat | 5:b8b1a979b0d5 | 248 | |
TheDarkDurzo | 8:1c8a747c49c8 | 249 | //turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser... |
TheDarkDurzo | 8:1c8a747c49c8 | 250 | |
ruesipat | 7:5ef09519a6e9 | 251 | //dontStop = way%10; |
ruesipat | 7:5ef09519a6e9 | 252 | //way = way/10; |
TheDarkDurzo | 8:1c8a747c49c8 | 253 | |
ruesipat | 5:b8b1a979b0d5 | 254 | |
ruesipat | 5:b8b1a979b0d5 | 255 | drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden |
ruesipat | 5:b8b1a979b0d5 | 256 | |
ruesipat | 5:b8b1a979b0d5 | 257 | if(blackLine == 1) { //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier. |
TheDarkDurzo | 8:1c8a747c49c8 | 258 | lab[startx][starty] = 5; |
ruesipat | 5:b8b1a979b0d5 | 259 | state = 2; |
ruesipat | 5:b8b1a979b0d5 | 260 | } |
ruesipat | 5:b8b1a979b0d5 | 261 | break; |
ruesipat | 5:b8b1a979b0d5 | 262 | } |
ruesipat | 5:b8b1a979b0d5 | 263 | case 2: { //Ziel erreicht und Wartemodus fuer 2. Lauf |
ruesipat | 5:b8b1a979b0d5 | 264 | |
ruesipat | 5:b8b1a979b0d5 | 265 | controller.setDesiredSpeedRight(0.0f); |
ruesipat | 5:b8b1a979b0d5 | 266 | controller.setDesiredSpeedLeft(0.0f); |
ruesipat | 5:b8b1a979b0d5 | 267 | |
TheDarkDurzo | 8:1c8a747c49c8 | 268 | startx = 0; |
TheDarkDurzo | 8:1c8a747c49c8 | 269 | starty = 9; |
TheDarkDurzo | 8:1c8a747c49c8 | 270 | |
TheDarkDurzo | 8:1c8a747c49c8 | 271 | |
TheDarkDurzo | 8:1c8a747c49c8 | 272 | //wait(30.4f); |
TheDarkDurzo | 8:1c8a747c49c8 | 273 | int num; |
TheDarkDurzo | 8:1c8a747c49c8 | 274 | c=0; |
TheDarkDurzo | 8:1c8a747c49c8 | 275 | //Speichert die 2-Dimensionale Karte in ein 1D-Array welche nun aus anweisungen besteht |
TheDarkDurzo | 8:1c8a747c49c8 | 276 | while(lab[startx][starty] != 5) { |
TheDarkDurzo | 8:1c8a747c49c8 | 277 | num = lab[startx][starty]; |
TheDarkDurzo | 8:1c8a747c49c8 | 278 | pathArr[c] = num; |
TheDarkDurzo | 8:1c8a747c49c8 | 279 | |
TheDarkDurzo | 8:1c8a747c49c8 | 280 | switch(num) { |
TheDarkDurzo | 8:1c8a747c49c8 | 281 | case 1: { |
TheDarkDurzo | 8:1c8a747c49c8 | 282 | starty--; |
TheDarkDurzo | 8:1c8a747c49c8 | 283 | break; |
TheDarkDurzo | 8:1c8a747c49c8 | 284 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 285 | case 2: { |
TheDarkDurzo | 8:1c8a747c49c8 | 286 | startx--; |
TheDarkDurzo | 8:1c8a747c49c8 | 287 | break; |
TheDarkDurzo | 8:1c8a747c49c8 | 288 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 289 | case 3: { |
TheDarkDurzo | 8:1c8a747c49c8 | 290 | startx++; |
TheDarkDurzo | 8:1c8a747c49c8 | 291 | break; |
TheDarkDurzo | 8:1c8a747c49c8 | 292 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 293 | case 4: { |
TheDarkDurzo | 8:1c8a747c49c8 | 294 | starty++; |
TheDarkDurzo | 8:1c8a747c49c8 | 295 | break; |
TheDarkDurzo | 8:1c8a747c49c8 | 296 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 297 | case 5: { |
TheDarkDurzo | 8:1c8a747c49c8 | 298 | |
TheDarkDurzo | 8:1c8a747c49c8 | 299 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 300 | }//Switch End |
TheDarkDurzo | 8:1c8a747c49c8 | 301 | |
TheDarkDurzo | 8:1c8a747c49c8 | 302 | myled =! myled; // LED is ON Heartbeat |
TheDarkDurzo | 8:1c8a747c49c8 | 303 | wait(0.4f); |
TheDarkDurzo | 8:1c8a747c49c8 | 304 | //printf("Array = %d , num = %d\n\r", pathArr[c],num); |
TheDarkDurzo | 8:1c8a747c49c8 | 305 | c++; |
TheDarkDurzo | 8:1c8a747c49c8 | 306 | }//While Ende |
TheDarkDurzo | 8:1c8a747c49c8 | 307 | |
TheDarkDurzo | 8:1c8a747c49c8 | 308 | pathArr[c]= 5; //setzt end bit |
TheDarkDurzo | 8:1c8a747c49c8 | 309 | |
TheDarkDurzo | 8:1c8a747c49c8 | 310 | |
TheDarkDurzo | 8:1c8a747c49c8 | 311 | c=0; |
TheDarkDurzo | 8:1c8a747c49c8 | 312 | |
TheDarkDurzo | 8:1c8a747c49c8 | 313 | |
TheDarkDurzo | 8:1c8a747c49c8 | 314 | |
TheDarkDurzo | 8:1c8a747c49c8 | 315 | //übersetzt den 1-Dimensionalen Array der Ausrichtung in einen Array nun Anweisungen zur Lösung darstellt |
TheDarkDurzo | 8:1c8a747c49c8 | 316 | while(pathArr[c]!= 5) { |
TheDarkDurzo | 8:1c8a747c49c8 | 317 | if(c==0) { |
TheDarkDurzo | 8:1c8a747c49c8 | 318 | if(pathArr[c]==1) { |
TheDarkDurzo | 8:1c8a747c49c8 | 319 | directions[c]=1; |
TheDarkDurzo | 8:1c8a747c49c8 | 320 | } else { |
TheDarkDurzo | 8:1c8a747c49c8 | 321 | directions[c]=3; |
TheDarkDurzo | 8:1c8a747c49c8 | 322 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 323 | //printf("pathArr: %d, directions: %d \r\n", pathArr[c], directions[c]); |
TheDarkDurzo | 8:1c8a747c49c8 | 324 | } else if(pathArr[c-1]==pathArr[c]) { |
TheDarkDurzo | 8:1c8a747c49c8 | 325 | directions[c]=1; |
TheDarkDurzo | 8:1c8a747c49c8 | 326 | //printf("pathArr: %d, directions: %d \r\n", pathArr[c], directions[c]); |
TheDarkDurzo | 8:1c8a747c49c8 | 327 | } else { |
TheDarkDurzo | 8:1c8a747c49c8 | 328 | |
TheDarkDurzo | 8:1c8a747c49c8 | 329 | if(pathArr[c]==1) { |
TheDarkDurzo | 8:1c8a747c49c8 | 330 | if(pathArr[c-1]==3) { |
TheDarkDurzo | 8:1c8a747c49c8 | 331 | directions[c]=2; |
TheDarkDurzo | 8:1c8a747c49c8 | 332 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 333 | if(pathArr[c-1]==2) { |
TheDarkDurzo | 8:1c8a747c49c8 | 334 | directions[c]=3; |
TheDarkDurzo | 8:1c8a747c49c8 | 335 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 336 | if(pathArr[c-1]==4) { |
TheDarkDurzo | 8:1c8a747c49c8 | 337 | directions[c-1]=4; |
TheDarkDurzo | 8:1c8a747c49c8 | 338 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 339 | } else if(pathArr[c]==2) { |
TheDarkDurzo | 8:1c8a747c49c8 | 340 | if(pathArr[c-1]==1) { |
TheDarkDurzo | 8:1c8a747c49c8 | 341 | directions[c]=2; |
TheDarkDurzo | 8:1c8a747c49c8 | 342 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 343 | if(pathArr[c-1]==3) { |
TheDarkDurzo | 8:1c8a747c49c8 | 344 | directions[c]=4; |
TheDarkDurzo | 8:1c8a747c49c8 | 345 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 346 | if(pathArr[c-1]==4) { |
TheDarkDurzo | 8:1c8a747c49c8 | 347 | directions[c]=3; |
TheDarkDurzo | 8:1c8a747c49c8 | 348 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 349 | } else if(pathArr[c]==3) { |
TheDarkDurzo | 8:1c8a747c49c8 | 350 | if(pathArr[c-1]==1) { |
TheDarkDurzo | 8:1c8a747c49c8 | 351 | directions[c]=3; |
TheDarkDurzo | 8:1c8a747c49c8 | 352 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 353 | if(pathArr[c-1]==2) { |
TheDarkDurzo | 8:1c8a747c49c8 | 354 | directions[c]=4; |
TheDarkDurzo | 8:1c8a747c49c8 | 355 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 356 | if(pathArr[c-1]==4) { |
TheDarkDurzo | 8:1c8a747c49c8 | 357 | directions[c]=2; |
TheDarkDurzo | 8:1c8a747c49c8 | 358 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 359 | } else if(pathArr[c]==4) { |
TheDarkDurzo | 8:1c8a747c49c8 | 360 | if(pathArr[c-1]==1) { |
TheDarkDurzo | 8:1c8a747c49c8 | 361 | directions[c]=4; |
TheDarkDurzo | 8:1c8a747c49c8 | 362 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 363 | if(pathArr[c-1]==2) { |
TheDarkDurzo | 8:1c8a747c49c8 | 364 | directions[c]=2; |
TheDarkDurzo | 8:1c8a747c49c8 | 365 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 366 | if(pathArr[c-1]==3) { |
TheDarkDurzo | 8:1c8a747c49c8 | 367 | directions[c]=3; |
TheDarkDurzo | 8:1c8a747c49c8 | 368 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 369 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 370 | //printf("pathArr: %d, directions: %d \r\n", pathArr[c], directions[c]); |
TheDarkDurzo | 8:1c8a747c49c8 | 371 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 372 | c++; |
TheDarkDurzo | 8:1c8a747c49c8 | 373 | myled =! myled; // LED is ON Heartbeat |
TheDarkDurzo | 8:1c8a747c49c8 | 374 | wait(0.1f); |
TheDarkDurzo | 8:1c8a747c49c8 | 375 | //printf("Array = %d \n\r", directions[c]); |
TheDarkDurzo | 8:1c8a747c49c8 | 376 | |
TheDarkDurzo | 8:1c8a747c49c8 | 377 | } |
TheDarkDurzo | 8:1c8a747c49c8 | 378 | c=0; |
TheDarkDurzo | 8:1c8a747c49c8 | 379 | |
ruesipat | 5:b8b1a979b0d5 | 380 | while (button) { |
ruesipat | 5:b8b1a979b0d5 | 381 | //printf("WARTE AUF 2.START..."); |
ruesipat | 5:b8b1a979b0d5 | 382 | wait(1.0f); |
ruesipat | 5:b8b1a979b0d5 | 383 | } |
ruesipat | 5:b8b1a979b0d5 | 384 | blackLine = 0; |
ruesipat | 5:b8b1a979b0d5 | 385 | state = 3; |
TheDarkDurzo | 8:1c8a747c49c8 | 386 | myled =0; // LED is ON Heartbeat |
TheDarkDurzo | 8:1c8a747c49c8 | 387 | |
TheDarkDurzo | 8:1c8a747c49c8 | 388 | |
TheDarkDurzo | 8:1c8a747c49c8 | 389 | c=0; |
ruesipat | 5:b8b1a979b0d5 | 390 | break; |
ruesipat | 5:b8b1a979b0d5 | 391 | } |
ruesipat | 5:b8b1a979b0d5 | 392 | case 3: {//2. Lauf Fahrzyklus |
TheDarkDurzo | 8:1c8a747c49c8 | 393 | //übergibt Anweisung an turn |
TheDarkDurzo | 8:1c8a747c49c8 | 394 | modeStart=1; |
TheDarkDurzo | 8:1c8a747c49c8 | 395 | |
TheDarkDurzo | 8:1c8a747c49c8 | 396 | path=directions[c]; |
TheDarkDurzo | 8:1c8a747c49c8 | 397 | |
TheDarkDurzo | 8:1c8a747c49c8 | 398 | printf("path %d, modeStart %d \r\n", path, modeStart); |
TheDarkDurzo | 8:1c8a747c49c8 | 399 | |
ruesipat | 5:b8b1a979b0d5 | 400 | //pruefug der waender entfaellt, da jetzt die daten aus dem speicher geladen werden |
ruesipat | 7:5ef09519a6e9 | 401 | //checkWalls.check(); //prueft wo Waende vorhanden sind |
ruesipat | 7:5ef09519a6e9 | 402 | // |
TheDarkDurzo | 8:1c8a747c49c8 | 403 | //integer fuer kuerzesten weg...3 rechts 1 gerade aus 2 links und 4 180 Grad drehung |
ruesipat | 7:5ef09519a6e9 | 404 | // wallRight = 0; |
ruesipat | 7:5ef09519a6e9 | 405 | // wallFront = 0; |
ruesipat | 7:5ef09519a6e9 | 406 | // wallLeft = 0; |
ruesipat | 5:b8b1a979b0d5 | 407 | |
ruesipat | 5:b8b1a979b0d5 | 408 | turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser... |
ruesipat | 5:b8b1a979b0d5 | 409 | |
ruesipat | 7:5ef09519a6e9 | 410 | //dontStop = 2; |
ruesipat | 5:b8b1a979b0d5 | 411 | |
ruesipat | 5:b8b1a979b0d5 | 412 | drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden |
TheDarkDurzo | 8:1c8a747c49c8 | 413 | c++; |
ruesipat | 5:b8b1a979b0d5 | 414 | if(blackLine == 1) { //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier. |
ruesipat | 5:b8b1a979b0d5 | 415 | |
ruesipat | 5:b8b1a979b0d5 | 416 | state = 4; |
ruesipat | 5:b8b1a979b0d5 | 417 | } |
ruesipat | 5:b8b1a979b0d5 | 418 | |
ruesipat | 5:b8b1a979b0d5 | 419 | break; |
ruesipat | 5:b8b1a979b0d5 | 420 | |
ruesipat | 5:b8b1a979b0d5 | 421 | } |
ruesipat | 5:b8b1a979b0d5 | 422 | case 4: { //Ziel erreicht und Wartemodus fuer 2. Lauf |
ruesipat | 5:b8b1a979b0d5 | 423 | |
ruesipat | 5:b8b1a979b0d5 | 424 | controller.setDesiredSpeedRight(0.0f); |
ruesipat | 5:b8b1a979b0d5 | 425 | controller.setDesiredSpeedLeft(0.0f); |
ruesipat | 5:b8b1a979b0d5 | 426 | |
ruesipat | 5:b8b1a979b0d5 | 427 | while (button) { |
ruesipat | 5:b8b1a979b0d5 | 428 | //printf("SCHLUSS..."); |
ruesipat | 5:b8b1a979b0d5 | 429 | wait(1.0f); |
ruesipat | 5:b8b1a979b0d5 | 430 | } |
ruesipat | 5:b8b1a979b0d5 | 431 | |
ruesipat | 5:b8b1a979b0d5 | 432 | state = 5; |
ruesipat | 5:b8b1a979b0d5 | 433 | break; |
ruesipat | 5:b8b1a979b0d5 | 434 | } |
ruesipat | 5:b8b1a979b0d5 | 435 | default: { |
ruesipat | 5:b8b1a979b0d5 | 436 | state = 0; |
ruesipat | 5:b8b1a979b0d5 | 437 | break; |
ruesipat | 5:b8b1a979b0d5 | 438 | } |
ruesipat | 0:a9fe4ef404bf | 439 | } |
ruesipat | 1:d9e840c48b1e | 440 | |
ruesipat | 1:d9e840c48b1e | 441 | |
ruesipat | 1:d9e840c48b1e | 442 | |
ruesipat | 0:a9fe4ef404bf | 443 | |
ruesipat | 5:b8b1a979b0d5 | 444 | //printf("DistanzRechts = %.0f mm\n\r", irSensor0.read()); |
ruesipat | 5:b8b1a979b0d5 | 445 | //printf("DistanzVorne = %.0f mm\n\r", irSensor1.read()); |
ruesipat | 5:b8b1a979b0d5 | 446 | //printf("DistanzLinksVorne = %.0f mm\n\r", irSensor2.read()); |
ruesipat | 5:b8b1a979b0d5 | 447 | //printf("DistanzLinksHinten = %.0f mm\n\r", irSensor3.read()); |
ruesipat | 5:b8b1a979b0d5 | 448 | //printf("WandRechts = %d \n\r", wallRight); |
ruesipat | 5:b8b1a979b0d5 | 449 | //printf("WandVorne = %d \n\r", wallFront); |
ruesipat | 5:b8b1a979b0d5 | 450 | //printf("WandLinks = %d \n\r", wallLeft); |
ruesipat | 2:592f01278db4 | 451 | |
ruesipat | 2:592f01278db4 | 452 | |
ruesipat | 5:b8b1a979b0d5 | 453 | // printf("CountsRight = %d\n", counterRight.read()); |
ruesipat | 5:b8b1a979b0d5 | 454 | // printf("CountsLeft = %d\n", counterLeft.read()); |
ruesipat | 5:b8b1a979b0d5 | 455 | //kontrastSensor.check(); |
ruesipat | 5:b8b1a979b0d5 | 456 | //printf("SchwarzeLinie = %d", blackLine); |
ruesipat | 5:b8b1a979b0d5 | 457 | myled =! myled; // LED is ON Heartbeat |
ruesipat | 5:b8b1a979b0d5 | 458 | wait(0.1f); |
ruesipat | 1:d9e840c48b1e | 459 | |
TheDarkDurzo | 8:1c8a747c49c8 | 460 | |
ruesipat | 2:592f01278db4 | 461 | |
ruesipat | 5:b8b1a979b0d5 | 462 | |
ruesipat | 0:a9fe4ef404bf | 463 | } |
ruesipat | 0:a9fe4ef404bf | 464 | } |