File content as of revision 7:3b3a2c158803:

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

using namespace std::chrono;


    DigitalIn user_button(USER_BUTTON);
    DigitalOut     led(LED1);
    
    // create servo objects
    Servo  servo_0(PA_6);
    
    
   //Infrarotsensor
    DigitalIn pirlinks(PB_12);
    DigitalIn pirhinten(PC_7);   //pc_6 puuty besetzt
    DigitalIn pirrechts(PB_7);  //pc_8
    DigitalIn pirvorne(PA_1); //pc-9
    //im moment nich eingebaut
  //  DigitalIn ultrarechts(PB_1);
   // DigitalIn ultralinks(PB_13); //immer eins behoben
   // DigitalIn ultravorne(PC_3); 
    //DigitalIn ultrahinten(PA_9);// immer eins behoben
    
    //Output
    //Servo
    DigitalOut pumpe(PB_6);
    DigitalOut summer(PA_0);
    
    // Groove Ultrasonic Ranger V2.0
    // https://ch.rs-online.com/web/p/entwicklungstools-sensorik/1743238/?cm_mmc=CH-PLA-DS3A-_-google-_-CSS_CH_DE_Raspberry_Pi_%26_Arduino_und_Entwicklungstools_Whoop-_-(CH:Whoop!)+Entwicklungstools+Sensorik-_-1743238&matchtype=&pla-306637898829&gclid=Cj0KCQjwpdqDBhCSARIsAEUJ0hOLQOOaw_2-Ob03u4YGwXthQPeSyjaazFqNuMkTIT8Ie18B1pD7P9AaAn18EALw_wcB&gclsrc=aw.ds 
    /* create range finder object (ultra sonic distance sensor) */
    RangeFinder ultrarechts(PB_1, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
    RangeFinder ultralinks(PB_13, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
    RangeFinder ultravorne(PC_3, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
    RangeFinder ultrahinten(PA_9, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
    

    /* input your stuff here */
    
Timer          loop_timer;
int            Ts_ms = 500;
char           CZustand = 0;
int            spritz = 3000; //spritzzeit
float          fhelp = 0.0f;



/*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(2100); //1250
       if(ultrarechts.read_cm() < 200)
       {
        fhelp = ultrarechts.read_cm();
        printf("%f;",fhelp);
        CZustand = 6;
        }
        else{
        CZustand = 0;
  
        }
        
        
        thread_sleep_for(100);
    }
    if(CZustand == 3)//links erkennt
    {
       printf("links\n");
       servo_0.SetPosition(1000);
        if(ultralinks.read_cm() < 200)
       {
        fhelp = ultralinks.read_cm();
        printf("%3.3f;\r\n",fhelp);
        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.read_cm() < 200)
       {
        CZustand = 6;
        }
        else{
        CZustand = 0;
        thread_sleep_for(100);  
        } 
        
        
        thread_sleep_for(100);
    }
        
    if(CZustand == 5)//hinten erkennt
    {
      printf("hinten\n");
      servo_0.SetPosition(2450);
       if(ultrahinten.read_cm() < 200) 
       {
        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);
        
    }
  
        
        
        
}