File content as of revision 6:9b34fadd6628:

#include "mbed.h"
#include "platform/mbed_thread.h"
#include "SDBlockDevice.h"
#include "FATFileSystem.h"
#include "EncoderCounter.h"
#include "Servo.h"
#include "SpeedController.h"

using namespace std::chrono;


    DigitalIn user_button(USER_BUTTON);
    DigitalOut     led(LED1);
    
    // create servo objects
    Servo  servo_0(PA_6);
    
    
   //Input 
    DigitalIn pirrechts(PB_12);
    DigitalIn pirlinks(PC_7);   //pc_6 puuty besetzt
    DigitalIn pirvorne(PB_7);  //pc_8
    DigitalIn pirhinten(PA_1); //pc-9
    DigitalIn ultrarechts(PB_1);
    DigitalIn ultralinks(PC_5); //immer eins
    DigitalIn ultravorne(PC_3); 
    DigitalIn ultrahinten(PC_2);// immer eins
    
    //Output
    //Servo
    DigitalOut pumpe(PB_6);
    DigitalOut summer(PA_0);
    
 
    /* input your stuff here */
    
Timer          loop_timer;
int            Ts_ms = 500;
char           CZustand = 55;
int            spritz = 3000; //spritzzeit



/*Interrupt*/

/*
class Counter {
public:
    Counter(PinName pin) : _interrupt(pin) {        // create the InterruptIn on the pin specified to Counter
        _interrupt.rise(this, &Counter::increment); // attach increment function of this counter instance
    }
 
    void increment() {
        _count++;
    }
 
    int read() {
        return _count;
    }
 
private:
    InterruptIn _interrupt;
    volatile int _count;
};
 
*/


int main()
{

 servo_0.Enable(1500,20000);
 
        

    
    while (true) {

     loop_timer.reset();
     if(user_button == 0)
     {
         printf("nicht aktiv");
         CZustand = 0;
         
         }
 
        /* ------------- start hacking ------------- -------------*/
    if(CZustand == 0) // Aufstarten
    {
        //servo_0.SetPosition(1000);
        printf("Grundposition\n");
        
        thread_sleep_for(100);  
        CZustand = 1; 
    }
    if(CZustand == 1)// Warten
    {
        led = 0;
        //printf("Warten\n");
        if(pirrechts == 1)
        {
            CZustand = 2;
            printf("erkennt\n");
            thread_sleep_for(100);
            //servo_0.SetPosition(2000);
            
        }
        if(pirvorne == 1)
        {
            CZustand = 4;
            }
        if(pirlinks == 1)
        {
            CZustand = 3;
        }
        if(pirhinten == 1)
        {
            printf("pirhinten\n");
            CZustand = 5;
        }
        
    
        
        
        
    }
    else{
        
        }
    
    if(CZustand == 2)//rechts erkennt
    {
       led = 1;
       printf("rechts\n");
       
       servo_0.SetPosition(1250); //1250
       if(ultrarechts == 1)
       {
        CZustand = 6;
        }
        else{
        CZustand = 0;
  
        }
        
        
        thread_sleep_for(100);
    }
    if(CZustand == 3)//links erkennt
    {
       printf("links\n");
       servo_0.SetPosition(1750);
        if(ultralinks == 1)
       {
        CZustand = 6;
        }
        else{
        CZustand = 0;
        //thread_sleep_for(100);  
        } 
        
        
        thread_sleep_for(100);
    }
    if(CZustand == 4)//vorne erkennt
    {
        printf("vorne\n");
       servo_0.SetPosition(1500);
       if(ultravorne == 1)
       {
        CZustand = 6;
        }
        else{
        CZustand = 0;
        thread_sleep_for(100);  
        } 
        
        
        thread_sleep_for(100);
    }
        
    if(CZustand == 5)//hinten erkennt
    {
      printf("hinten\n");
      servo_0.SetPosition(2000);
       if(ultrahinten == 1) 
       {
        CZustand = 6;
        }
        else{
        CZustand = 0;
         
        } 
        
        thread_sleep_for(100);   
    }
    if(CZustand == 6)//spritzen
    {
        printf("spritzen");
        summer = 1;
        pumpe = 1;
        thread_sleep_for(spritz); // Spritzzeit
        summer = 0;
        pumpe = 0;
        CZustand = 0;
        servo_0.SetPosition(1500); 
        

    }
 
        /* ------------- stop hacking ------------- -------------*/
 
        int T_loop_ms = duration_cast<milliseconds>(loop_timer.elapsed_time()).count();
        int dT_loop_ms = Ts_ms - T_loop_ms;
        thread_sleep_for(dT_loop_ms);
        
    }
  
        
        
        
}