---

Dependencies:   mbed

Fork of MicroMouse_MASTER_FIVE by PES2_R2D2.0

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); 

        }
    }
}