PES2_R2D2.0 / Mbed 2 deprecated MicroMouse_MASTER_TWO

READY TO RUMBLE

Dependencies:   mbed

Fork of Micromouse_alpha_copy_copy by PES2_R2D2.0

main.cpp@2:592f01278db4, 2018-04-04 (annotated)

Committer:
ruesipat
Date:
Wed Apr 04 15:24:28 2018 +0000
Revision:
2:592f01278db4
Parent:
1:d9e840c48b1e
Child:
3:2ec7cf8bc3fc
p

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 1:d9e840c48b1e 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
ruesipat 1:d9e840c48b1e 70 //Turn turn(); //GEHT NICHT!!!
ruesipat 1:d9e840c48b1e 71 int state = 0;
ruesipat 1:d9e840c48b1e 72 int mode = 1;
ruesipat 1:d9e840c48b1e 73
ruesipat 1:d9e840c48b1e 74 int main()
ruesipat 1:d9e840c48b1e 75 {
ruesipat 1:d9e840c48b1e 76
ruesipat 0:a9fe4ef404bf 77
ruesipat 2:592f01278db4 78
ruesipat 0:a9fe4ef404bf 79
ruesipat 1:d9e840c48b1e 80 enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
ruesipat 1:d9e840c48b1e 81 enable = 1;
ruesipat 0:a9fe4ef404bf 82
ruesipat 2:592f01278db4 83
ruesipat 1:d9e840c48b1e 84 while(1) { // Wiederholungsschleife
ruesipat 0:a9fe4ef404bf 85
ruesipat 1:d9e840c48b1e 86 //Ablauf ist jeweils immer Feldweise...Waende checken, allenfalls drehen (Ausrichten), waehrend der Fahrt nur Ausrichtung der Geradeausfahrt...
ruesipat 0:a9fe4ef404bf 87
ruesipat 2:592f01278db4 88 //printf("WARTE AUF START....");
ruesipat 1:d9e840c48b1e 89
ruesipat 1:d9e840c48b1e 90 while (button) {
ruesipat 2:592f01278db4 91 wait(1.0f); //Die Ruhe vor dem Sturm...
ruesipat 0:a9fe4ef404bf 92 }
ruesipat 1:d9e840c48b1e 93
ruesipat 1:d9e840c48b1e 94
ruesipat 1:d9e840c48b1e 95
ruesipat 1:d9e840c48b1e 96 while(mode==1) {
ruesipat 0:a9fe4ef404bf 97
ruesipat 1:d9e840c48b1e 98 //Ermittlung der Abstaende rund ums Fahrzeug
ruesipat 1:d9e840c48b1e 99 float distanceRight = irSensor0.read();
ruesipat 1:d9e840c48b1e 100 float distanceFront = irSensor1.read();
ruesipat 1:d9e840c48b1e 101 float distanceLeftFront = irSensor2.read();
ruesipat 1:d9e840c48b1e 102 float distanceLeftBack = irSensor3.read();
ruesipat 1:d9e840c48b1e 103
ruesipat 1:d9e840c48b1e 104 int wallRight;
ruesipat 1:d9e840c48b1e 105 int wallFront;
ruesipat 1:d9e840c48b1e 106 int wallLeft;
ruesipat 1:d9e840c48b1e 107 int blackLine;
ruesipat 1:d9e840c48b1e 108
ruesipat 1:d9e840c48b1e 109 CheckWalls checkWalls(distanceRight, distanceFront, distanceLeftFront, wallRight, wallFront, wallLeft); //Ermittlung wo freie Wege(0) bzw welche Waende vorhanden (1)
ruesipat 1:d9e840c48b1e 110 //Decide??
ruesipat 1:d9e840c48b1e 111 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 1:d9e840c48b1e 112 KontrastSensor kontrastSensor(kontrast, blackLine);
ruesipat 2:592f01278db4 113 Drive drive(kontrastSensor, counterLeft, counterRight, controller, distanceRight, distanceFront, distanceLeftFront, distanceLeftBack,); //20cm nach vorne...
ruesipat 2:592f01278db4 114
ruesipat 1:d9e840c48b1e 115
ruesipat 1:d9e840c48b1e 116 switch(state) { //Ausrichten
ruesipat 1:d9e840c48b1e 117 case 0: {
ruesipat 1:d9e840c48b1e 118
ruesipat 1:d9e840c48b1e 119
ruesipat 1:d9e840c48b1e 120
ruesipat 1:d9e840c48b1e 121 state = 1;
ruesipat 1:d9e840c48b1e 122 break;
ruesipat 1:d9e840c48b1e 123 }
ruesipat 1:d9e840c48b1e 124 case 1: {//Fahrzyklus
ruesipat 1:d9e840c48b1e 125
ruesipat 2:592f01278db4 126 checkWalls.check(); //prueft wo Waende vorhanden sind
ruesipat 1:d9e840c48b1e 127
ruesipat 1:d9e840c48b1e 128 //wallRight = 1;
ruesipat 1:d9e840c48b1e 129 //wallFront = 1;
ruesipat 1:d9e840c48b1e 130 //wallLeft = 1;
ruesipat 1:d9e840c48b1e 131
ruesipat 2:592f01278db4 132 turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
ruesipat 1:d9e840c48b1e 133
ruesipat 2:592f01278db4 134
ruesipat 2:592f01278db4 135 drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden
ruesipat 1:d9e840c48b1e 136
ruesipat 2:592f01278db4 137
ruesipat 1:d9e840c48b1e 138
ruesipat 2:592f01278db4 139 //blackLine = 1;
ruesipat 1:d9e840c48b1e 140 if(blackLine == 1){ //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier.
ruesipat 1:d9e840c48b1e 141
ruesipat 2:592f01278db4 142 state = 2;
ruesipat 1:d9e840c48b1e 143 }
ruesipat 1:d9e840c48b1e 144 break;
ruesipat 1:d9e840c48b1e 145 }
ruesipat 1:d9e840c48b1e 146 case 2: { //Ziel erreicht
ruesipat 1:d9e840c48b1e 147
ruesipat 2:592f01278db4 148 controller.setDesiredSpeedRight(0.0f);
ruesipat 2:592f01278db4 149 controller.setDesiredSpeedLeft(0.0f);
ruesipat 1:d9e840c48b1e 150
ruesipat 1:d9e840c48b1e 151 //"Stop"
ruesipat 1:d9e840c48b1e 152 //mode=2
ruesipat 1:d9e840c48b1e 153 break;
ruesipat 1:d9e840c48b1e 154 }
ruesipat 1:d9e840c48b1e 155 default: { //
ruesipat 1:d9e840c48b1e 156 state = 0;
ruesipat 1:d9e840c48b1e 157 break;
ruesipat 1:d9e840c48b1e 158 }
ruesipat 1:d9e840c48b1e 159 }
ruesipat 1:d9e840c48b1e 160
ruesipat 2:592f01278db4 161
ruesipat 2:592f01278db4 162
ruesipat 2:592f01278db4 163
ruesipat 2:592f01278db4 164 printf("DistanzRechts = %.0f mm\n\r", distanceRight);
ruesipat 2:592f01278db4 165 printf("DistanzVorne = %.0f mm\n\r", distanceFront);
ruesipat 2:592f01278db4 166 printf("DistanzLinksVorne = %.0f mm\n\r", distanceLeftFront);
ruesipat 2:592f01278db4 167 printf("DistanzLinksHinten = %.0f mm\n\r", distanceLeftBack);
ruesipat 2:592f01278db4 168 printf("WandRechts = %d \n\r", wallRight);
ruesipat 2:592f01278db4 169 printf("WandVorne = %d \n\r", wallFront);
ruesipat 2:592f01278db4 170 printf("WandLinks = %d \n\r", wallLeft);
ruesipat 2:592f01278db4 171
ruesipat 2:592f01278db4 172
ruesipat 2:592f01278db4 173 // printf("CountsRight = %d\n", counterRight.read());
ruesipat 2:592f01278db4 174 // printf("CountsLeft = %d\n", counterLeft.read());
ruesipat 1:d9e840c48b1e 175 //kontrastSensor.check();
ruesipat 1:d9e840c48b1e 176 //printf("SchwarzeLinie = %d", blackLine);
ruesipat 1:d9e840c48b1e 177 myled =! myled; // LED is ON Heartbeat
ruesipat 2:592f01278db4 178 // wait(0.5f);
ruesipat 1:d9e840c48b1e 179
ruesipat 2:592f01278db4 180 }//Ende while mode==1
ruesipat 2:592f01278db4 181
ruesipat 2:592f01278db4 182
ruesipat 0:a9fe4ef404bf 183 }
ruesipat 0:a9fe4ef404bf 184 }