PES2_R2D2.0 / Mbed 2 deprecated MicroMouse_MASTER_THREE

mit pathdingsbums

Dependencies:   mbed

Fork of MicroMouse_MASTER_TWO by PES2_R2D2.0

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 
mit pathfinding

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 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 }