![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Version12.04.18
Fork of Micromouse_beta by
Diff: main.cpp
- Revision:
- 1:d9e840c48b1e
- Parent:
- 0:a9fe4ef404bf
- Child:
- 2:592f01278db4
--- a/main.cpp Wed Mar 07 14:06:19 2018 +0000 +++ b/main.cpp Sat Mar 31 16:45:57 2018 +0000 @@ -1,136 +1,178 @@ #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" -DigitalIn button(USER_BUTTON); -DigitalOut myled(LED1); -DigitalOut led0(PC_8); -DigitalOut led1(PC_6); -DigitalOut led2(PB_12); -DigitalOut led3(PA_7); -DigitalOut led4(PC_0); -DigitalOut led5(PC_9); + +//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); -DigitalIn motorDriverFault(PB_14); -DigitalIn motorDriverWarning(PB_15); - -PwmOut pwmLeft(PA_8); -PwmOut pwmRight(PA_9); +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 -EncoderCounter controllerLeft(PB_6, PB_7); -EncoderCounter controllerRight(PA_6, PC_7); - -Controller controller(pwmLeft, pwmRight, controllerLeft, controllerRight); - - -//AnalogIn analog_value(A0); - DigitalOut enable(PC_1); +Controller controller(pwmLeft, pwmRight, counterLeft, counterRight); -AnalogIn distance(PB_1); //Kreieren der Ein - und Ausgangsobjekte -DigitalOut bit0(PH_1); -DigitalOut bit1(PC_2); -DigitalOut bit2(PC_3); + +//Turn turn(); //GEHT NICHT!!! +int state = 0; +int mode = 1; + +int main() +{ + -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); -IRSensor irSensor4(distance, bit0, bit1, bit2, 4); -IRSensor irSensor5(distance, bit0, bit1, bit2, 5); + enableMotorDriver = 1; // Schaltet den Leistungstreiber ein + enable = 1; -int main() { + while(1) { // Wiederholungsschleife - - enableMotorDriver = 1; // Schaltet den Leistungstreiber ein - - enable = 1; + //Ablauf ist jeweils immer Feldweise...Waende checken, allenfalls drehen (Ausrichten), waehrend der Fahrt nur Ausrichtung der Geradeausfahrt... - while(1) { - - - - float distance0 = irSensor0.read(); - float distance1 = irSensor1.read(); - float distance2 = irSensor2.read(); - float distance3 = irSensor3.read(); - float distance4 = irSensor4.read(); - float distance5 = irSensor5.read(); - - - if ( distance0 < 0.1f){ - led0 = 1; - controller.setDesiredSpeedLeft(0.0f); //Drehzahl in (rpm) - controller.setDesiredSpeedRight(0.0f); //Drehzahl in (rpm) - } - else{ - led0 = 0; - controller.setDesiredSpeedLeft(50.0f); - controller.setDesiredSpeedRight(-50.0f); + printf("WARTE AUF START...."); + + while (button) { + wait(1.0f); //Die Ruhe vor dem Sturm... } - - if ( distance1 < 0.1f){ - led1 = 1; - controller.setDesiredSpeedLeft(-50.0f); - // controller.setDesiredSpeedRight(0.0f); - } - else{ - led1 = 0; - // controller.setDesiredSpeedLeft(0.0f); - // controller.setDesiredSpeedRight(0.0f); - } - if ( distance2 < 0.1f){ - led2 = 1; - //pwmLeft = 0.6; - //pwmRight = 0.4; - } - else{ - led2 = 0; - //pwmLeft = 0.5; - //pwmRight = 0.5; - } - if ( distance3 < 0.1f){ - led3 = 1; - //pwmLeft = 0.6; - //pwmRight = 0.4; - } - else{ - led3 = 0; - //pwmLeft = 0.5; - //pwmRight = 0.5; - } - if ( distance4 < 0.1f){ - led4 = 1; - //pwmLeft = 0.6; - //pwmRight = 0.4; - } - else{ - led4 = 0; - //pwmLeft = 0.5; - //pwmRight = 0.5; - } - - if ( distance5 < 0.1f){ - led5 = 1; - controller.setDesiredSpeedRight(50.0f); - //pwmRight = 0.4; - } - else{ - led5 = 0; - //pwmLeft = 0.5; - //pwmRight = 0.5; - } - - myled =! myled; // LED is ON - wait(0.1f); // 100 ms + + + + 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); + + } } }