![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
mit pathdingsbums
Dependencies: mbed
Fork of MicroMouse_MASTER_TWO by
main.cpp
- Committer:
- ruesipat
- Date:
- 2018-03-31
- Revision:
- 1:d9e840c48b1e
- Parent:
- 0:a9fe4ef404bf
- Child:
- 2:592f01278db4
File content as of revision 1:d9e840c48b1e:
#include "mbed.h" #include "IRSensor.h" #include "EncoderCounter.h" #include "LowpassFilter.h" #include "Controller.h" #include "KontrastSensor.h" #include "Drive.h" #include "CheckWalls.h" #include "Turn.h" #include "Parallel.h" //Initialisierung LEDs Blinker/Surri/Button DigitalIn button(USER_BUTTON); //Moduswählknopf DigitalOut myled(LED1); //Heartbeat (evt auch Anzeige für Modus, vor start DigitalOut blinker0(PA_4); //Blinker links DigitalOut blinker1(PC_1); //Blinker rechts DigitalOut surri(PC_0); // //Initialisierung LEDs Nightrider PwmOut led0(PB_0); //von links nach rechts PwmOut led1(PB_8); PwmOut led2(PB_3); PwmOut led3(PB_5); PwmOut led4(PB_4); PwmOut led5(PB_10); PwmOut led6(PB_9); //Initialisierung IR-Sensoren AnalogIn distance0(PC_2); //Kreieren der Eingangsobjekte AnalogIn distance1(PC_3); AnalogIn distance2(PC_5); AnalogIn distance3(PB_1); IRSensor irSensor0(distance0); //rechts IRSensor irSensor1(distance1); //vorne IRSensor irSensor2(distance2); //links-vorne IRSensor irSensor3(distance3); //links-hinten //AnalogIn distance(PB_1); //Kreieren der Ein - und Ausgangsobjekte //DigitalOut bit0(PH_1); //DigitalOut bit1(PC_2); //DigitalOut bit2(PC_3); //IRSensor irSensor0(distance, bit0, bit1, bit2, 0); //IRSensor irSensor1(distance, bit0, bit1, bit2, 1); //IRSensor irSensor2(distance, bit0, bit1, bit2, 2); //IRSensor irSensor3(distance, bit0, bit1, bit2, 3); //Initialisierung Kontrastsensor AnalogIn kontrast(PC_0); //Initialisierung Motor und Encoder DigitalOut enableMotorDriver(PB_2); PwmOut pwmLeft(PA_8); //Motor links PwmOut pwmRight(PA_9); //Motor rechts EncoderCounter counterLeft(PB_6, PB_7); //Encoder für Motor links EncoderCounter counterRight(PA_6, PC_7);//Encoder für Motor rechts DigitalOut enable(PC_1); Controller controller(pwmLeft, pwmRight, counterLeft, counterRight); //Turn turn(); //GEHT NICHT!!! int state = 0; int mode = 1; int main() { enableMotorDriver = 1; // Schaltet den Leistungstreiber ein enable = 1; while(1) { // Wiederholungsschleife //Ablauf ist jeweils immer Feldweise...Waende checken, allenfalls drehen (Ausrichten), waehrend der Fahrt nur Ausrichtung der Geradeausfahrt... printf("WARTE AUF START...."); while (button) { wait(1.0f); //Die Ruhe vor dem Sturm... } while(mode==1) { //Ermittlung der Abstaende rund ums Fahrzeug float distanceRight = irSensor0.read(); float distanceFront = irSensor1.read(); float distanceLeftFront = irSensor2.read(); float distanceLeftBack = irSensor3.read(); int wallRight; int wallFront; int wallLeft; int blackLine; CheckWalls checkWalls(distanceRight, distanceFront, distanceLeftFront, wallRight, wallFront, wallLeft); //Ermittlung wo freie Wege(0) bzw welche Waende vorhanden (1) //Decide?? Turn turn(counterLeft, counterRight, controller, wallRight, wallFront, wallLeft); //Nach Ausrichtung bewegt sich der Roboter um ein Feld weiter und die Ausrichutung beginnt von vorne KontrastSensor kontrastSensor(kontrast, blackLine); Drive drive(kontrastSensor, counterLeft, counterRight, controller, distanceLeftFront, distanceLeftBack, distanceFront); //20cm nach vorne... Parallel parallel(distanceLeftFront, distanceLeftBack); switch(state) { //Ausrichten case 0: { state = 1; break; } case 1: {//Fahrzyklus //checkWalls.check(); //prueft wo Waende vorhanden sind //wallRight = 1; //wallFront = 1; //wallLeft = 1; //turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser... controller.setDesiredSpeedRight(0.5f); controller.setDesiredSpeedLeft(-0.5f); //drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden blackLine = 1; if(blackLine == 1){ //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier. state = 2; } break; } case 2: { //Ziel erreicht //"Stop" //mode=2 break; } default: { // state = 0; break; } } 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()); //printf("DistanzRechts = %.0f mm\n", distanceRight); //printf("DistanzVorne = %.0f mm\n", distanceFront); // printf("DistanzLinksVorne = %.0f mm\n", distanceLeftFront); // printf("DistanzLinksHinten = %.0f mm\n", distanceLeftBack); // printf("WandRechts = %d \n", wallRight); // printf("WandVorne = %d \n", wallFront); // printf("WandLinks = %d \n", wallLeft); //printf("CountsRight = %d\n", counterRight.read()); //printf("CountsLeft = %d\n", counterLeft.read()); //kontrastSensor.check(); //printf("SchwarzeLinie = %d", blackLine); myled =! myled; // LED is ON Heartbeat wait(0.5f); } } }