File content as of revision 4:52685928a7c3:

#include "mbed.h"
#include "Sensor_auslesen.h"
#include "cstdlib"
#include "Servo.h"

DigitalOut enable(PC_1);
DigitalOut bit0(PH_1);
DigitalOut bit1(PC_2);
DigitalOut bit2(PC_3);

DigitalOut enableMotorDriver(PB_2);
PwmOut pwmLeft(PA_8);
PwmOut pwmRight(PA_9);
IRSensor sensors[6];
Servo Servo1(PB_7);

//LED's
DigitalOut led0(PC_8);
DigitalOut led1(PC_6);
DigitalOut led5(PC_9);
DigitalOut led3(PC_7);

// Analog In
AnalogIn distance(PB_1);
AnalogIn Sensor1(PA_0);
AnalogIn Sensor2(PA_1);

//Variablen definieren*********************************************************
int robot_state;
enum robot_state {gerade=0,rechts,links,drehen,zurueck,zurueck_l,zurueck_r,Lego_R,Lego_G};
int zustand =0;           // variable to store the value Green
enum zustand {rot=0, gruen};
int merker_gruen = 0;
int merker_gruen1 = 0;
double wenden =0.26;
double wand =0.07;
int einschlatZeit = 2;
int ausschaltZeit =0;

int main()
{
    //Motoren Bremsen 
    
 pwmLeft=0.5;
 pwmRight=0.5;


//Sensoren auslesen*********************************************************
    for( int ii = 0; ii<6; ++ii) {
        sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii);

        enable = 1;


        pwmLeft.period(0.00005);
        pwmRight.period(0.00005);

        enableMotorDriver =1;
    }

   
    float valInG = 0;
    float valInR = 0;

 
    while( 1 ) {
        



//Farbsensor auslesen + Auswertung***************************************************

        valInG = Sensor1*100.0;
        valInR = Sensor2*100.0;
        
     
            
        
        
        


//State setzen****************************************************************
        switch (robot_state) {

          /*  case gerade:
                led0=1;
                led1=0;
                led3=0;
                led5=0;
                break;

            case rechts:
                led0=0;
                led1=1;
                led3=0;
                led5=0;
                break;

            case links:
                led0=0;
                led1=0;
                led3=0;
                led5=1;
                break;

            case drehen:
                led0=1;
                led1=1;
                led3=0;
                led5=0;
                break;

            case zurueck:
                led0=0;
                led1=0;
                led3=1;
                led5=0;
                */
            case rot:
                led0=1;
                led1=1;
                led3=1;
                led5=1;
                break;
            
            case gruen:
                led0=1;
                led1=0;
                led3=1;
                led5=0;
                break;
            
        }
//Sensoren abfrgagen und Motoren stellen***************************************


// Wenn Front + Front-Right + Front-Left etwas sehen => gerade zurück bis nur noch einer etwas sieht
        if(sensors[0]<=0.1 && sensors[1]<=0.12 && sensors[5]<=0.12) {
           

            //wenn hinten rechts => leichte linkskurve
            if(sensors[2]<=0.25) {
                pwmLeft=0.35;
                pwmRight=0.55;
                printf("zurueck-linkskurve\n");
                robot_state=zurueck_l;
            }

            if(sensors[4]<=0.25) {
                pwmLeft=0.35;
                pwmRight=0.55;
                printf("zurueck-rechtskurve\n");
                robot_state=zurueck_r;
            }
            if(sensors[4]>=0.25 && sensors[2]>=0.25) {
                pwmLeft=0.4;
                pwmRight=0.6;
                robot_state=zurueck;
                printf("zurueck-gerade\n");
            }
        }
      
        // Wenn Front etwas sehen => drehen***********************************
        
       else if(sensors[1] <= wand || sensors[5]<=wand){
        pwmLeft=0.35;
        pwmRight=0.65;
        }
        
        
        
        else if(sensors[0]<0.25 && sensors [1]<=wenden) {  
           pwmLeft=0.4;
           pwmRight=0.4;   
           printf("drehen links\n\n\n");
        }  
                    
        else if (sensors[0]<0.25 && sensors [5]<=wenden){    
            pwmLeft=0.6;
            pwmRight=0.6;   
            printf("drehen\n\n\n");
               }
        else if(sensors[0]<0.25) {
        if (rand()%2==0 && robot_state != drehen) {
                pwmLeft=0.4;
                pwmRight=0.4;
            } else if (rand()%2 != 0 && robot_state != drehen) {
                pwmLeft=0.6;
                pwmRight=0.6;
            }
        robot_state=drehen;
        }


        //Wenn Front-Left etwas sehen => nach Rechts**************************
        else if(sensors[5]<=wenden) {
            printf("rechts\n");
            pwmLeft=0.65;
            pwmRight=0.45;
            robot_state=rechts;
        }

        // Wenn Front-Right etwas sehen => Links*******************************
        else if(sensors[1]<=wenden) {
            printf("Links\n");
            pwmLeft=0.55;
            pwmRight=0.35;
            robot_state=links;
        }

        //Wenn kein Sensor anspricht => gerade aus*****************************
        else if(sensors[0]>=0.26) {
            printf("gerade\n");
            pwmLeft=0.65;
            pwmRight=0.35;
            robot_state=gerade;
        }
        
    // Auswurf ansteuern******************************************************* 
   
    
       if ((valInG>=70.5 && valInG<=77.0 && valInR>=52.5 && valInR<=55) || (valInG>=77.8 && valInG<=79 && valInR>=73 && valInR<=74) || (valInG>=76 && valInG<=78.2 && valInR>=64.6 && valInR<=65.5)){      
           if(zustand==rot){
            ausschaltZeit+=5;
            }
            zustand = gruen;
            if(merker_gruen==0){
            merker_gruen=1;
            }
        }
            else{
                zustand = rot;
            }
           
           

        
        if(zustand == gruen || (merker_gruen >=1 && merker_gruen < einschlatZeit)){ //Einschaltverzögerung
            merker_gruen ++;
            robot_state=Lego_G;
        }
        if (merker_gruen1 == ausschaltZeit){                            //Merker zurücksetzen
            merker_gruen1=0;
            }
        
        else if(merker_gruen == einschlatZeit || (merker_gruen1 >=1 && merker_gruen1 < ausschaltZeit)){ // Ausschaltverzögerung
            merker_gruen =0;
            merker_gruen1 ++;
             Servo1 = 1.4f;
             robot_state=Lego_G;
            }
            
        else if(zustand == rot && merker_gruen == 0 && merker_gruen1 ==0){
            Servo1 =0.1f;      //Grenze 0..7.4
            robot_state=Lego_R;
            ausschaltZeit = 0;
            }
         
    //Ausgaben an Konsole******************************************************

        printf("\n\r");
       // printf("front:%f\n\r", sensors[0].read());
        //printf("right:%f\n\r", sensors[1].read());
        //printf("left:%f\n\r", sensors[5].read());
       printf("Gruen:%f\n\r", valInG);
       printf("Rot:%f\n\n\r", valInR);
       printf("Status:%d\n\r",zustand);
      // printf("Merker1:%d\n\r",merker_gruen);
      // printf("Merker2:%d\n\r",merker_gruen1);
      // printf("Merker2:%d\n\r",ausschaltZeit);

        wait(0.5);

    }

}