File content as of revision 0:66569508393e:

#include "mbed.h"
#include "IRSensor.h"
#include "EncoderCounter.h"
#include "LowpassFilter.h"
#include "Controller.h"
#include "Motion.h"

//Serial
Serial pc(USBTX, USBRX);
AnalogIn distance(PB_1); // Kreieren der Ein- und Ausgangsobjekte 
DigitalOut enable(PC_1);
DigitalOut bit0(PH_1);
DigitalOut bit1(PC_2);
DigitalOut bit2(PC_3);

//LED 
DigitalOut led0(PC_8); 
DigitalOut led1(PC_6); 
DigitalOut led2(PB_12); 
DigitalOut led3(PA_7); 
DigitalOut led4(PC_0); 
DigitalOut led5(PC_9);

//Ansteuerung
DigitalOut enableMotorDriver(PB_2); 
DigitalIn motorDriverFault(PB_14); 
DigitalIn motorDriverWarning(PB_15); 
PwmOut pwmLeft(PA_8);
PwmOut pwmRight(PA_9);

DigitalIn  Button(USER_BUTTON);



int main() {
    //Entcoder
    EncoderCounter counterLeft(PB_6, PB_7); 
    EncoderCounter counterRight(PA_6, PC_7);
    Controller controller(pwmLeft, pwmRight, counterLeft, counterRight); 
    int init=0;
int state=1;
        
    while(1) {
        
        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);
        // Objekte kreieren
        enable = 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();
        
        switch (state){
                case 1:
                    enableMotorDriver = 0; // Schaltet den Leistungstreiber aus
                      
                    if (Button==0){
                        enableMotorDriver = 1; state=2;
                        }
                        //if (init==1){enableMotorDriver = 0; state=2;}
                        
                    break;
                case 2:
                        controller.setTranslationalVelocity(-5);
                        if (distance3 <= 0.2){
                          state=6;
                             }
                        if (Button==0){
                            state=5;
                        } 
                        if (distance4 <=0.2){
                            state=4;
                             controller.setTranslationalVelocity(0);
                        }
                        if (distance2 <=0.2){
                            state=3;
                             controller.setTranslationalVelocity(0);
                        }
                    break;    
                case 3:
                        controller.setRotationalVelocity(-0.1);
                         if (Button==0){
                            state=5;controller.setRotationalVelocity(0);
                        } 
                      
                        if (distance2 <=0.2){
                            state=3;
                        }else { controller.setRotationalVelocity(0); state=2;}
                    break;
                case 4:
                    controller.setRotationalVelocity(0.1);
                             if (Button==0){
                                state=5;controller.setRotationalVelocity(0);
                            } 
                            if (distance4 <=0.2){
                                state=4;
                            } else { controller.setRotationalVelocity(0); state=2;}

                        break;
                case 5:
                        controller.setTranslationalVelocity(0);
                        state=1;
                    break;
                    case 6:
                            controller.setTranslationalVelocity(5);
                            wait(1);
                            controller.setTranslationalVelocity(0);
                            controller.setRotationalVelocity(0.1);
                            wait(0.2);
                            controller.setRotationalVelocity(0);
                            state =2;
                            break;
            }
       

    }
}