---

Dependencies:   mbed

Fork of MicroMouse_MASTER_FIVE by PES2_R2D2.0

main.cpp

Committer:
ruesipat
Date:
2018-04-04
Revision:
2:592f01278db4
Parent:
1:d9e840c48b1e
Child:
3:2ec7cf8bc3fc

File content as of revision 2:592f01278db4:

#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"



//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, distanceRight, distanceFront, distanceLeftFront, distanceLeftBack,); //20cm nach vorne...
                                                           
            
            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...
                    

                    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
                    
                        controller.setDesiredSpeedRight(0.0f);
                        controller.setDesiredSpeedLeft(0.0f);
                    
                    //"Stop"
                    //mode=2
                    break;
                }
                default: { //
                    state = 0;
                    break;
                }
            }

                    


            printf("DistanzRechts = %.0f mm\n\r", distanceRight);
            printf("DistanzVorne = %.0f mm\n\r", distanceFront);
            printf("DistanzLinksVorne = %.0f mm\n\r", distanceLeftFront);
            printf("DistanzLinksHinten = %.0f mm\n\r", distanceLeftBack);
            printf("WandRechts = %d \n\r", wallRight);
            printf("WandVorne = %d \n\r", wallFront);
            printf("WandLinks = %d \n\r", 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); 

        }//Ende while mode==1
        

    }
}