File content as of revision 16:0a1703438e8b:

#include <mbed.h>
#include "Roboter.h"

DigitalOut led(LED1);

//Periphery for distance sensors
AnalogIn distance(PB_1);
DigitalOut enable(PC_1);
DigitalOut bit0(PH_1);
DigitalOut bit1(PC_2);
DigitalOut bit2(PC_3);
IRSensor sensors[6];

// Periphery for servos
Servo servos1(PB_7);
Servo servos2(PA_6);

//Periphery for limit switch
DigitalIn limitSwitch1(PA_10);
DigitalIn limitSwitch2(PB_13);

//Periphery for Switch ON/OFF

DigitalIn switchOnOff(USER_BUTTON);

//motor stuff
DigitalOut enableMotorDriver(PB_2);
PwmOut pwmL( PA_8 );
PwmOut pwmR( PA_9 );

//indicator leds arround robot
DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };

Roboter roboter1(&distance, &bit0, &bit1, &bit2, &pwmL, &pwmR, &servos1,&servos2, &limitSwitch1, &limitSwitch2);

int main()
{
    enable = 1;  // Sensoren einschalten
    enableMotorDriver = 1; // Schaltet die Motoren ein
    pwmL.period(0.00005f); // Setzt die Periode auf 50 μs
    pwmR.period(0.00005f);
    pwmL = 0.5f;
    pwmR = 0.5f;


    int state = 0; // Diese Variable gibt an in welchem State man sich befindet
    int tempState = 2;
    int time1 = 0;
    int time2 = 0;

    while(1) {

        int camValue = readCamera();

        switch(state) {

            case 0: // Roboter Anschalten mit Knopf
                printf("0");

                if (switchOnOff == 0) {
                    state = 1;
                }

                break;

            case 1:
                printf("1");
                if(camValue == 1 || camValue == 2 || camValue == 3) {           // Die Kamera erkennt ein grünen Klotz
                    state = 4;
                }
                if(camValue == 4) {     // Roboter in Position
                    state = 5;
                }
                if(camValue == 0){
                    state = tempState;
                }
                break;

            case 2:
                printf("2");
                if(time1 < 20) {       // Im Kreis drehen für 1s
                    pwmL = 0.4f;
                    pwmR = 0.4f;
                    time1 ++;
                    state = 1;
                    tempState = 2;
                } else {
                    time1 = 0;
                    pwmL = 0.5f;
                    pwmR = 0.5f;
                    state = 3;
                }
                break;


            case 3:
                printf("3");
                roboter1.bandeAusweichen();
                if(time2 < 40) {    // gerade aus fahren
                    pwmL = 0.6f;
                    pwmR = 0.4f;
                    time2 ++;
                    state = 1;
                    tempState = 3;
                } else {
                    time2 = 0;
                    pwmL = 0.5f;
                    pwmR = 0.5f;
                    state = 2;
                }
                break;


            case 4: // Roboter erkennt gründer Klotz, Klotz wird angefahren
                printf("4");
                roboter1.bandeAusweichen();
                if(camValue == 1) {     // links fahren
                    pwmL = 0.45f;
                    pwmR = 0.45f;
                }
                if(camValue == 2) {     // rechts fahren
                    pwmL = 0.55f;
                    pwmR = 0.55f;
                }
                if(camValue == 3) {     // gerade fahren
                    pwmL = 0.55f;
                    pwmR = 0.45f;
                }
                state = 1;

                break;

            case 5: // Aufnehmen des Klotzes
                printf("5");
                pwmL = 0.5f;
                pwmR = 0.5f;
                state = 1;
                time1 = 0;
                time2 = 0;

                break;


            default:
                break;


        }
        
        //wait(0.1f);
    }

}