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@1:d9e840c48b1e, 2018-03-31 (annotated)

Committer:
ruesipat
Date:
Sat Mar 31 16:45:57 2018 +0000
Revision:
1:d9e840c48b1e
Parent:
0:a9fe4ef404bf
Child:
2:592f01278db4
j

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