PES2_R2D2.0 / Mbed 2 deprecated MicroMouse_MASTER_SIX

---

Dependencies:   mbed

Fork of MicroMouse_MASTER_FIVE by PES2_R2D2.0

main.cpp@5:b8b1a979b0d5, 2018-04-25 (annotated)

Committer:
ruesipat
Date:
Wed Apr 25 12:07:03 2018 +0000
Revision:
5:b8b1a979b0d5
Parent:
4:e3f388933954
Child:
7:5ef09519a6e9
25.04.2018_FINDET ZIEL

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 0:a9fe4ef404bf 12
ruesipat 1:d9e840c48b1e 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
ruesipat 0:a9fe4ef404bf 31
ruesipat 1:d9e840c48b1e 32 //Initialisierung IR-Sensoren
ruesipat 1:d9e840c48b1e 33 AnalogIn distance0(PC_2); //Kreieren der Eingangsobjekte
ruesipat 1:d9e840c48b1e 34 AnalogIn distance1(PC_3);
ruesipat 1:d9e840c48b1e 35 AnalogIn distance2(PC_5);
ruesipat 1:d9e840c48b1e 36 AnalogIn distance3(PB_1);
ruesipat 1:d9e840c48b1e 37 IRSensor irSensor0(distance0); //rechts
ruesipat 1:d9e840c48b1e 38 IRSensor irSensor1(distance1); //vorne
ruesipat 1:d9e840c48b1e 39 IRSensor irSensor2(distance2); //links-vorne
ruesipat 5:b8b1a979b0d5 40 IRSensor irSensor3(distance3); //links-hinten
ruesipat 1:d9e840c48b1e 41
ruesipat 1:d9e840c48b1e 42
ruesipat 1:d9e840c48b1e 43 //AnalogIn distance(PB_1); //Kreieren der Ein - und Ausgangsobjekte
ruesipat 1:d9e840c48b1e 44 //DigitalOut bit0(PH_1);
ruesipat 1:d9e840c48b1e 45 //DigitalOut bit1(PC_2);
ruesipat 1:d9e840c48b1e 46 //DigitalOut bit2(PC_3);
ruesipat 1:d9e840c48b1e 47 //IRSensor irSensor0(distance, bit0, bit1, bit2, 0);
ruesipat 1:d9e840c48b1e 48 //IRSensor irSensor1(distance, bit0, bit1, bit2, 1);
ruesipat 1:d9e840c48b1e 49 //IRSensor irSensor2(distance, bit0, bit1, bit2, 2);
ruesipat 1:d9e840c48b1e 50 //IRSensor irSensor3(distance, bit0, bit1, bit2, 3);
ruesipat 1:d9e840c48b1e 51
ruesipat 1:d9e840c48b1e 52
ruesipat 1:d9e840c48b1e 53 //Initialisierung Kontrastsensor
ruesipat 1:d9e840c48b1e 54 AnalogIn kontrast(PC_0);
ruesipat 1:d9e840c48b1e 55
ruesipat 1:d9e840c48b1e 56
ruesipat 1:d9e840c48b1e 57
ruesipat 1:d9e840c48b1e 58 //Initialisierung Motor und Encoder
ruesipat 0:a9fe4ef404bf 59 DigitalOut enableMotorDriver(PB_2);
ruesipat 1:d9e840c48b1e 60 PwmOut pwmLeft(PA_8); //Motor links
ruesipat 1:d9e840c48b1e 61 PwmOut pwmRight(PA_9); //Motor rechts
ruesipat 1:d9e840c48b1e 62 EncoderCounter counterLeft(PB_6, PB_7); //Encoder für Motor links
ruesipat 1:d9e840c48b1e 63 EncoderCounter counterRight(PA_6, PC_7);//Encoder für Motor rechts
ruesipat 0:a9fe4ef404bf 64
ruesipat 0:a9fe4ef404bf 65 DigitalOut enable(PC_1);
ruesipat 0:a9fe4ef404bf 66
ruesipat 1:d9e840c48b1e 67 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
ruesipat 0:a9fe4ef404bf 68
ruesipat 1:d9e840c48b1e 69 int state = 0;
ruesipat 1:d9e840c48b1e 70
ruesipat 1:d9e840c48b1e 71 int main()
ruesipat 1:d9e840c48b1e 72 {
ruesipat 1:d9e840c48b1e 73
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 2:592f01278db4 81
ruesipat 5:b8b1a979b0d5 82 // 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 0:a9fe4ef404bf 83
ruesipat 5:b8b1a979b0d5 84 CheckWalls checkWalls(irSensor0, irSensor1, irSensor2, wallRight, wallFront, wallLeft); //Ermittlung wo freie Wege(0) bzw welche Waende vorhanden (1)
ruesipat 5:b8b1a979b0d5 85 Turn turn(counterLeft, counterRight, controller, wallRight, wallFront, wallLeft); //Nach Ausrichtung bewegt sich der Roboter um ein Feld weiter und die Ausrichutung beginnt von vorne
ruesipat 5:b8b1a979b0d5 86 KontrastSensor kontrastSensor(kontrast, blackLine);
ruesipat 5:b8b1a979b0d5 87 Drive drive(kontrastSensor, counterLeft, counterRight, controller, irSensor0, irSensor1, irSensor2, irSensor3); //20cm nach vorne...//Ablauf ist jeweils immer Feldweise...Waende checken, allenfalls drehen (Ausrichten), waehrend der Fahrt nur Ausrichtung der Geradeausfahrt...
ruesipat 0:a9fe4ef404bf 88
ruesipat 2:592f01278db4 89
ruesipat 1:d9e840c48b1e 90 while(1) { // Wiederholungsschleife
ruesipat 0:a9fe4ef404bf 91
ruesipat 5:b8b1a979b0d5 92
ruesipat 5:b8b1a979b0d5 93 switch(state) { //Wartemodus 1. Lauf
ruesipat 5:b8b1a979b0d5 94 case 0: {
ruesipat 5:b8b1a979b0d5 95
ruesipat 5:b8b1a979b0d5 96
ruesipat 5:b8b1a979b0d5 97 while (button) {
ruesipat 5:b8b1a979b0d5 98 //printf("WARTE AUF 1.START...");
ruesipat 5:b8b1a979b0d5 99 wait(1.0f); //Die Ruhe vor dem Sturm...
ruesipat 5:b8b1a979b0d5 100 }
ruesipat 5:b8b1a979b0d5 101
ruesipat 5:b8b1a979b0d5 102 state = 1;
ruesipat 5:b8b1a979b0d5 103 break;
ruesipat 5:b8b1a979b0d5 104 }
ruesipat 5:b8b1a979b0d5 105 case 1: {// 1.Lauf Fahrzyklus und Aufzeichungszyklus
ruesipat 5:b8b1a979b0d5 106
ruesipat 5:b8b1a979b0d5 107 checkWalls.check(); //prueft wo Waende vorhanden sind
ruesipat 5:b8b1a979b0d5 108
ruesipat 5:b8b1a979b0d5 109 //wallRight = 1;
ruesipat 5:b8b1a979b0d5 110 //wallFront = 1;
ruesipat 5:b8b1a979b0d5 111 //wallLeft = 1;
ruesipat 5:b8b1a979b0d5 112
ruesipat 5:b8b1a979b0d5 113 turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
ruesipat 5:b8b1a979b0d5 114
ruesipat 5:b8b1a979b0d5 115
ruesipat 5:b8b1a979b0d5 116 //saveWay
ruesipat 5:b8b1a979b0d5 117 //Abspeicherung der Waende...
ruesipat 5:b8b1a979b0d5 118 // if(wallRight == 1){
ruesipat 5:b8b1a979b0d5 119 // way[0][0][1] = 1;
ruesipat 5:b8b1a979b0d5 120 // } else {
ruesipat 5:b8b1a979b0d5 121 // way[0][0][1] = 0;
ruesipat 5:b8b1a979b0d5 122 // }
ruesipat 5:b8b1a979b0d5 123 //
ruesipat 5:b8b1a979b0d5 124 // if(wallRight == 1){
ruesipat 5:b8b1a979b0d5 125 // way[0][0][2] = 1;
ruesipat 5:b8b1a979b0d5 126 // } else{
ruesipat 5:b8b1a979b0d5 127 // way[0][0][2] = 0;
ruesipat 5:b8b1a979b0d5 128 // }
ruesipat 5:b8b1a979b0d5 129 //
ruesipat 5:b8b1a979b0d5 130 // if(wallRight == 1){
ruesipat 5:b8b1a979b0d5 131 // way[0][0][3] = 1;
ruesipat 5:b8b1a979b0d5 132 // } else{
ruesipat 5:b8b1a979b0d5 133 // way[0][0][3] = 0;
ruesipat 5:b8b1a979b0d5 134 // }
ruesipat 5:b8b1a979b0d5 135 //Abspeicherung des entschiedenen Wegs und Anpassung des
ruesipat 5:b8b1a979b0d5 136
ruesipat 5:b8b1a979b0d5 137
ruesipat 5:b8b1a979b0d5 138 // printf("%d", way[0][0][0]);
ruesipat 5:b8b1a979b0d5 139
ruesipat 5:b8b1a979b0d5 140 drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden
ruesipat 5:b8b1a979b0d5 141
ruesipat 5:b8b1a979b0d5 142
ruesipat 5:b8b1a979b0d5 143
ruesipat 5:b8b1a979b0d5 144 //blackLine = 1;
ruesipat 5:b8b1a979b0d5 145 if(blackLine == 1) { //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier.
ruesipat 1:d9e840c48b1e 146
ruesipat 5:b8b1a979b0d5 147 state = 2;
ruesipat 5:b8b1a979b0d5 148 }
ruesipat 5:b8b1a979b0d5 149 break;
ruesipat 5:b8b1a979b0d5 150 }
ruesipat 5:b8b1a979b0d5 151 case 2: { //Ziel erreicht und Wartemodus fuer 2. Lauf
ruesipat 5:b8b1a979b0d5 152
ruesipat 5:b8b1a979b0d5 153 controller.setDesiredSpeedRight(0.0f);
ruesipat 5:b8b1a979b0d5 154 controller.setDesiredSpeedLeft(0.0f);
ruesipat 5:b8b1a979b0d5 155
ruesipat 5:b8b1a979b0d5 156 while (button) {
ruesipat 5:b8b1a979b0d5 157 //printf("WARTE AUF 2.START...");
ruesipat 5:b8b1a979b0d5 158 wait(1.0f);
ruesipat 5:b8b1a979b0d5 159 }
ruesipat 5:b8b1a979b0d5 160 blackLine = 0;
ruesipat 5:b8b1a979b0d5 161 state = 3;
ruesipat 5:b8b1a979b0d5 162 break;
ruesipat 5:b8b1a979b0d5 163 }
ruesipat 5:b8b1a979b0d5 164 case 3: {//2. Lauf Fahrzyklus
ruesipat 5:b8b1a979b0d5 165
ruesipat 5:b8b1a979b0d5 166
ruesipat 5:b8b1a979b0d5 167 //pruefug der waender entfaellt, da jetzt die daten aus dem speicher geladen werden
ruesipat 5:b8b1a979b0d5 168 checkWalls.check(); //prueft wo Waende vorhanden sind
ruesipat 5:b8b1a979b0d5 169
ruesipat 5:b8b1a979b0d5 170 turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
ruesipat 5:b8b1a979b0d5 171
ruesipat 5:b8b1a979b0d5 172
ruesipat 5:b8b1a979b0d5 173
ruesipat 5:b8b1a979b0d5 174 drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden
ruesipat 5:b8b1a979b0d5 175
ruesipat 5:b8b1a979b0d5 176 if(blackLine == 1) { //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier.
ruesipat 5:b8b1a979b0d5 177
ruesipat 5:b8b1a979b0d5 178 state = 4;
ruesipat 5:b8b1a979b0d5 179 }
ruesipat 5:b8b1a979b0d5 180
ruesipat 5:b8b1a979b0d5 181 break;
ruesipat 5:b8b1a979b0d5 182
ruesipat 5:b8b1a979b0d5 183 }
ruesipat 5:b8b1a979b0d5 184 case 4: { //Ziel erreicht und Wartemodus fuer 2. Lauf
ruesipat 5:b8b1a979b0d5 185
ruesipat 5:b8b1a979b0d5 186 controller.setDesiredSpeedRight(0.0f);
ruesipat 5:b8b1a979b0d5 187 controller.setDesiredSpeedLeft(0.0f);
ruesipat 5:b8b1a979b0d5 188
ruesipat 5:b8b1a979b0d5 189 while (button) {
ruesipat 5:b8b1a979b0d5 190 //printf("SCHLUSS...");
ruesipat 5:b8b1a979b0d5 191 wait(1.0f);
ruesipat 5:b8b1a979b0d5 192 }
ruesipat 5:b8b1a979b0d5 193
ruesipat 5:b8b1a979b0d5 194 state = 5;
ruesipat 5:b8b1a979b0d5 195 break;
ruesipat 5:b8b1a979b0d5 196 }
ruesipat 5:b8b1a979b0d5 197 default: {
ruesipat 5:b8b1a979b0d5 198 state = 0;
ruesipat 5:b8b1a979b0d5 199 break;
ruesipat 5:b8b1a979b0d5 200 }
ruesipat 0:a9fe4ef404bf 201 }
ruesipat 1:d9e840c48b1e 202
ruesipat 1:d9e840c48b1e 203
ruesipat 1:d9e840c48b1e 204
ruesipat 0:a9fe4ef404bf 205
ruesipat 5:b8b1a979b0d5 206 //printf("DistanzRechts = %.0f mm\n\r", irSensor0.read());
ruesipat 5:b8b1a979b0d5 207 //printf("DistanzVorne = %.0f mm\n\r", irSensor1.read());
ruesipat 5:b8b1a979b0d5 208 //printf("DistanzLinksVorne = %.0f mm\n\r", irSensor2.read());
ruesipat 5:b8b1a979b0d5 209 //printf("DistanzLinksHinten = %.0f mm\n\r", irSensor3.read());
ruesipat 5:b8b1a979b0d5 210 //printf("WandRechts = %d \n\r", wallRight);
ruesipat 5:b8b1a979b0d5 211 //printf("WandVorne = %d \n\r", wallFront);
ruesipat 5:b8b1a979b0d5 212 //printf("WandLinks = %d \n\r", wallLeft);
ruesipat 2:592f01278db4 213
ruesipat 2:592f01278db4 214
ruesipat 5:b8b1a979b0d5 215 // printf("CountsRight = %d\n", counterRight.read());
ruesipat 5:b8b1a979b0d5 216 // printf("CountsLeft = %d\n", counterLeft.read());
ruesipat 5:b8b1a979b0d5 217 //kontrastSensor.check();
ruesipat 5:b8b1a979b0d5 218 //printf("SchwarzeLinie = %d", blackLine);
ruesipat 5:b8b1a979b0d5 219 myled =! myled; // LED is ON Heartbeat
ruesipat 5:b8b1a979b0d5 220 wait(0.1f);
ruesipat 1:d9e840c48b1e 221
ruesipat 2:592f01278db4 222
ruesipat 2:592f01278db4 223
ruesipat 5:b8b1a979b0d5 224
ruesipat 0:a9fe4ef404bf 225 }
ruesipat 0:a9fe4ef404bf 226 }