PES2_R2D2.0 / Mbed 2 deprecated MicroMouse_MASTER_FOUR

Pathfinding nach rechts funktioniert noch nicht...der rest schon

Dependencies:   mbed

Fork of MicroMouse_MASTER_THREE by PES2_R2D2.0

main.cpp@7:5ef09519a6e9, 2018-05-07 (annotated)

Committer:
ruesipat
Date:
Mon May 07 18:52:04 2018 +0000
Revision:
7:5ef09519a6e9
Parent:
5:b8b1a979b0d5
Child:
8:1c8a747c49c8
VERSION 7.5.18

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
ruesipat 1:d9e840c48b1e 12 //Initialisierung LEDs Blinker/Surri/Button
ruesipat 1:d9e840c48b1e 13 DigitalIn button(USER_BUTTON); //Moduswählknopf
ruesipat 1:d9e840c48b1e 14 DigitalOut myled(LED1); //Heartbeat (evt auch Anzeige für Modus, vor start
ruesipat 1:d9e840c48b1e 15 DigitalOut blinker0(PA_4); //Blinker links
ruesipat 1:d9e840c48b1e 16 DigitalOut blinker1(PC_1); //Blinker rechts
ruesipat 1:d9e840c48b1e 17 DigitalOut surri(PC_0); //
ruesipat 1:d9e840c48b1e 18
ruesipat 1:d9e840c48b1e 19 //Initialisierung LEDs Nightrider
ruesipat 1:d9e840c48b1e 20 PwmOut led0(PB_0); //von links nach rechts
ruesipat 1:d9e840c48b1e 21 PwmOut led1(PB_8);
ruesipat 1:d9e840c48b1e 22 PwmOut led2(PB_3);
ruesipat 1:d9e840c48b1e 23 PwmOut led3(PB_5);
ruesipat 1:d9e840c48b1e 24 PwmOut led4(PB_4);
ruesipat 1:d9e840c48b1e 25 PwmOut led5(PB_10);
ruesipat 1:d9e840c48b1e 26 PwmOut led6(PB_9);
ruesipat 1:d9e840c48b1e 27
ruesipat 1:d9e840c48b1e 28 //Initialisierung IR-Sensoren
ruesipat 1:d9e840c48b1e 29 AnalogIn distance0(PC_2); //Kreieren der Eingangsobjekte
ruesipat 1:d9e840c48b1e 30 AnalogIn distance1(PC_3);
ruesipat 1:d9e840c48b1e 31 AnalogIn distance2(PC_5);
ruesipat 1:d9e840c48b1e 32 AnalogIn distance3(PB_1);
ruesipat 1:d9e840c48b1e 33 IRSensor irSensor0(distance0); //rechts
ruesipat 1:d9e840c48b1e 34 IRSensor irSensor1(distance1); //vorne
ruesipat 1:d9e840c48b1e 35 IRSensor irSensor2(distance2); //links-vorne
ruesipat 5:b8b1a979b0d5 36 IRSensor irSensor3(distance3); //links-hinten
ruesipat 1:d9e840c48b1e 37
ruesipat 1:d9e840c48b1e 38 //Initialisierung Kontrastsensor
ruesipat 1:d9e840c48b1e 39 AnalogIn kontrast(PC_0);
ruesipat 1:d9e840c48b1e 40
ruesipat 1:d9e840c48b1e 41 //Initialisierung Motor und Encoder
ruesipat 0:a9fe4ef404bf 42 DigitalOut enableMotorDriver(PB_2);
ruesipat 1:d9e840c48b1e 43 PwmOut pwmLeft(PA_8); //Motor links
ruesipat 1:d9e840c48b1e 44 PwmOut pwmRight(PA_9); //Motor rechts
ruesipat 1:d9e840c48b1e 45 EncoderCounter counterLeft(PB_6, PB_7); //Encoder für Motor links
ruesipat 1:d9e840c48b1e 46 EncoderCounter counterRight(PA_6, PC_7);//Encoder für Motor rechts
ruesipat 0:a9fe4ef404bf 47
ruesipat 0:a9fe4ef404bf 48 DigitalOut enable(PC_1);
ruesipat 0:a9fe4ef404bf 49
ruesipat 1:d9e840c48b1e 50 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
ruesipat 0:a9fe4ef404bf 51
ruesipat 1:d9e840c48b1e 52 int state = 0;
ruesipat 1:d9e840c48b1e 53
ruesipat 1:d9e840c48b1e 54 int main()
ruesipat 1:d9e840c48b1e 55 {
ruesipat 1:d9e840c48b1e 56
ruesipat 5:b8b1a979b0d5 57 enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
ruesipat 5:b8b1a979b0d5 58 enable = 1;
ruesipat 5:b8b1a979b0d5 59
ruesipat 4:e3f388933954 60 int wallRight = 0;
ruesipat 4:e3f388933954 61 int wallFront = 0;
ruesipat 5:b8b1a979b0d5 62 int wallLeft = 0;
ruesipat 4:e3f388933954 63 int blackLine = 0;
ruesipat 7:5ef09519a6e9 64 int dontStop = 0;
ruesipat 7:5ef09519a6e9 65 //int way = 220;
ruesipat 2:592f01278db4 66
ruesipat 5:b8b1a979b0d5 67 // 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
ruesipat 7:5ef09519a6e9 68
ruesipat 7:5ef09519a6e9 69 //Funktionsdeklarationen
ruesipat 5:b8b1a979b0d5 70 CheckWalls checkWalls(irSensor0, irSensor1, irSensor2, wallRight, wallFront, wallLeft); //Ermittlung wo freie Wege(0) bzw welche Waende vorhanden (1)
ruesipat 7:5ef09519a6e9 71 Turn turn(counterLeft, counterRight, controller, wallRight, wallFront, wallLeft, dontStop); //Nach Ausrichtung bewegt sich der Roboter um ein Feld weiter und die Ausrichutung beginnt von vorne
ruesipat 5:b8b1a979b0d5 72 KontrastSensor kontrastSensor(kontrast, blackLine);
ruesipat 7:5ef09519a6e9 73 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 74
ruesipat 2:592f01278db4 75
ruesipat 1:d9e840c48b1e 76 while(1) { // Wiederholungsschleife
ruesipat 0:a9fe4ef404bf 77
ruesipat 5:b8b1a979b0d5 78
ruesipat 5:b8b1a979b0d5 79 switch(state) { //Wartemodus 1. Lauf
ruesipat 5:b8b1a979b0d5 80 case 0: {
ruesipat 5:b8b1a979b0d5 81
ruesipat 5:b8b1a979b0d5 82
ruesipat 5:b8b1a979b0d5 83 while (button) {
ruesipat 5:b8b1a979b0d5 84 //printf("WARTE AUF 1.START...");
ruesipat 7:5ef09519a6e9 85 //controller.setDesiredSpeedRight(0.5f);
ruesipat 7:5ef09519a6e9 86 //controller.setDesiredSpeedLeft(-0.5f);
ruesipat 5:b8b1a979b0d5 87 wait(1.0f); //Die Ruhe vor dem Sturm...
ruesipat 5:b8b1a979b0d5 88 }
ruesipat 5:b8b1a979b0d5 89
ruesipat 5:b8b1a979b0d5 90 state = 1;
ruesipat 5:b8b1a979b0d5 91 break;
ruesipat 5:b8b1a979b0d5 92 }
ruesipat 5:b8b1a979b0d5 93 case 1: {// 1.Lauf Fahrzyklus und Aufzeichungszyklus
ruesipat 5:b8b1a979b0d5 94
ruesipat 7:5ef09519a6e9 95
ruesipat 5:b8b1a979b0d5 96 checkWalls.check(); //prueft wo Waende vorhanden sind
ruesipat 5:b8b1a979b0d5 97
ruesipat 5:b8b1a979b0d5 98 turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
ruesipat 5:b8b1a979b0d5 99
ruesipat 5:b8b1a979b0d5 100
ruesipat 5:b8b1a979b0d5 101 //saveWay
ruesipat 5:b8b1a979b0d5 102 //Abspeicherung der Waende...
ruesipat 5:b8b1a979b0d5 103 // if(wallRight == 1){
ruesipat 5:b8b1a979b0d5 104 // way[0][0][1] = 1;
ruesipat 5:b8b1a979b0d5 105 // } else {
ruesipat 5:b8b1a979b0d5 106 // way[0][0][1] = 0;
ruesipat 5:b8b1a979b0d5 107 // }
ruesipat 5:b8b1a979b0d5 108 //
ruesipat 5:b8b1a979b0d5 109 // if(wallRight == 1){
ruesipat 5:b8b1a979b0d5 110 // way[0][0][2] = 1;
ruesipat 5:b8b1a979b0d5 111 // } else{
ruesipat 5:b8b1a979b0d5 112 // way[0][0][2] = 0;
ruesipat 5:b8b1a979b0d5 113 // }
ruesipat 5:b8b1a979b0d5 114 //
ruesipat 5:b8b1a979b0d5 115 // if(wallRight == 1){
ruesipat 5:b8b1a979b0d5 116 // way[0][0][3] = 1;
ruesipat 5:b8b1a979b0d5 117 // } else{
ruesipat 5:b8b1a979b0d5 118 // way[0][0][3] = 0;
ruesipat 5:b8b1a979b0d5 119 // }
ruesipat 5:b8b1a979b0d5 120 //Abspeicherung des entschiedenen Wegs und Anpassung des
ruesipat 7:5ef09519a6e9 121 //dontStop = way%10;
ruesipat 7:5ef09519a6e9 122 //way = way/10;
ruesipat 7:5ef09519a6e9 123
ruesipat 5:b8b1a979b0d5 124
ruesipat 5:b8b1a979b0d5 125 drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden
ruesipat 5:b8b1a979b0d5 126
ruesipat 5:b8b1a979b0d5 127 if(blackLine == 1) { //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier.
ruesipat 1:d9e840c48b1e 128
ruesipat 5:b8b1a979b0d5 129 state = 2;
ruesipat 5:b8b1a979b0d5 130 }
ruesipat 5:b8b1a979b0d5 131 break;
ruesipat 5:b8b1a979b0d5 132 }
ruesipat 5:b8b1a979b0d5 133 case 2: { //Ziel erreicht und Wartemodus fuer 2. Lauf
ruesipat 5:b8b1a979b0d5 134
ruesipat 5:b8b1a979b0d5 135 controller.setDesiredSpeedRight(0.0f);
ruesipat 5:b8b1a979b0d5 136 controller.setDesiredSpeedLeft(0.0f);
ruesipat 5:b8b1a979b0d5 137
ruesipat 5:b8b1a979b0d5 138 while (button) {
ruesipat 5:b8b1a979b0d5 139 //printf("WARTE AUF 2.START...");
ruesipat 5:b8b1a979b0d5 140 wait(1.0f);
ruesipat 5:b8b1a979b0d5 141 }
ruesipat 5:b8b1a979b0d5 142 blackLine = 0;
ruesipat 5:b8b1a979b0d5 143 state = 3;
ruesipat 5:b8b1a979b0d5 144 break;
ruesipat 5:b8b1a979b0d5 145 }
ruesipat 5:b8b1a979b0d5 146 case 3: {//2. Lauf Fahrzyklus
ruesipat 5:b8b1a979b0d5 147
ruesipat 5:b8b1a979b0d5 148
ruesipat 5:b8b1a979b0d5 149 //pruefug der waender entfaellt, da jetzt die daten aus dem speicher geladen werden
ruesipat 7:5ef09519a6e9 150 //checkWalls.check(); //prueft wo Waende vorhanden sind
ruesipat 7:5ef09519a6e9 151 //
ruesipat 7:5ef09519a6e9 152 //integer fuer kuerzesten weg...1 rechts 2 gerade aus 3 links
ruesipat 7:5ef09519a6e9 153 // wallRight = 0;
ruesipat 7:5ef09519a6e9 154 // wallFront = 0;
ruesipat 7:5ef09519a6e9 155 // wallLeft = 0;
ruesipat 5:b8b1a979b0d5 156
ruesipat 5:b8b1a979b0d5 157 turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
ruesipat 5:b8b1a979b0d5 158
ruesipat 7:5ef09519a6e9 159 //dontStop = 2;
ruesipat 5:b8b1a979b0d5 160
ruesipat 5:b8b1a979b0d5 161 drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden
ruesipat 5:b8b1a979b0d5 162
ruesipat 5:b8b1a979b0d5 163 if(blackLine == 1) { //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier.
ruesipat 5:b8b1a979b0d5 164
ruesipat 5:b8b1a979b0d5 165 state = 4;
ruesipat 5:b8b1a979b0d5 166 }
ruesipat 5:b8b1a979b0d5 167
ruesipat 5:b8b1a979b0d5 168 break;
ruesipat 5:b8b1a979b0d5 169
ruesipat 5:b8b1a979b0d5 170 }
ruesipat 5:b8b1a979b0d5 171 case 4: { //Ziel erreicht und Wartemodus fuer 2. Lauf
ruesipat 5:b8b1a979b0d5 172
ruesipat 5:b8b1a979b0d5 173 controller.setDesiredSpeedRight(0.0f);
ruesipat 5:b8b1a979b0d5 174 controller.setDesiredSpeedLeft(0.0f);
ruesipat 5:b8b1a979b0d5 175
ruesipat 5:b8b1a979b0d5 176 while (button) {
ruesipat 5:b8b1a979b0d5 177 //printf("SCHLUSS...");
ruesipat 5:b8b1a979b0d5 178 wait(1.0f);
ruesipat 5:b8b1a979b0d5 179 }
ruesipat 5:b8b1a979b0d5 180
ruesipat 5:b8b1a979b0d5 181 state = 5;
ruesipat 5:b8b1a979b0d5 182 break;
ruesipat 5:b8b1a979b0d5 183 }
ruesipat 5:b8b1a979b0d5 184 default: {
ruesipat 5:b8b1a979b0d5 185 state = 0;
ruesipat 5:b8b1a979b0d5 186 break;
ruesipat 5:b8b1a979b0d5 187 }
ruesipat 0:a9fe4ef404bf 188 }
ruesipat 1:d9e840c48b1e 189
ruesipat 1:d9e840c48b1e 190
ruesipat 1:d9e840c48b1e 191
ruesipat 0:a9fe4ef404bf 192
ruesipat 5:b8b1a979b0d5 193 //printf("DistanzRechts = %.0f mm\n\r", irSensor0.read());
ruesipat 5:b8b1a979b0d5 194 //printf("DistanzVorne = %.0f mm\n\r", irSensor1.read());
ruesipat 5:b8b1a979b0d5 195 //printf("DistanzLinksVorne = %.0f mm\n\r", irSensor2.read());
ruesipat 5:b8b1a979b0d5 196 //printf("DistanzLinksHinten = %.0f mm\n\r", irSensor3.read());
ruesipat 5:b8b1a979b0d5 197 //printf("WandRechts = %d \n\r", wallRight);
ruesipat 5:b8b1a979b0d5 198 //printf("WandVorne = %d \n\r", wallFront);
ruesipat 5:b8b1a979b0d5 199 //printf("WandLinks = %d \n\r", wallLeft);
ruesipat 2:592f01278db4 200
ruesipat 2:592f01278db4 201
ruesipat 5:b8b1a979b0d5 202 // printf("CountsRight = %d\n", counterRight.read());
ruesipat 5:b8b1a979b0d5 203 // printf("CountsLeft = %d\n", counterLeft.read());
ruesipat 5:b8b1a979b0d5 204 //kontrastSensor.check();
ruesipat 5:b8b1a979b0d5 205 //printf("SchwarzeLinie = %d", blackLine);
ruesipat 5:b8b1a979b0d5 206 myled =! myled; // LED is ON Heartbeat
ruesipat 5:b8b1a979b0d5 207 wait(0.1f);
ruesipat 1:d9e840c48b1e 208
ruesipat 2:592f01278db4 209
ruesipat 2:592f01278db4 210
ruesipat 5:b8b1a979b0d5 211
ruesipat 0:a9fe4ef404bf 212 }
ruesipat 0:a9fe4ef404bf 213 }