File content as of revision 8:2ae54c6a9dc6:

#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); //PA_6
    
    
   //Infrarotsensor
    DigitalIn pirrechts(PB_12);
    DigitalIn pirvorne(PC_7);  
    DigitalIn pirhinten(PB_7);  
    DigitalIn pirlinks(PA_1); 
    
  /*
  pc-6 pc_ 9 sicher mit putty besetzt pc_8 immer eins
  
  
  */
    

    
    // 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(PC_5, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
    RangeFinder ultralinks(PC_3, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
    RangeFinder ultravorne(PB_1, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
    RangeFinder ultrahinten(PC_2, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
    
    //Output
    //Servo
    DigitalOut pumpe(PB_6);
    DigitalOut summer(PA_0); 

    /* input your stuff here */
    
Timer          loop_timer;
int            Ts_ms = 1000;
char           CZustand = 0;
int            spritz = 1000; //spritzzeit, summerzeit = 2*spritzzeit
//float           dist_USSensor = 0.0f;



/*Interrupt nciht verwendet möglich erweiterung*/

/*
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);
 
 summer =1;
 thread_sleep_for(200);
 summer = 0;
//thread_sleep_for(15000);
 
        

    
    while (true) {

     loop_timer.reset();
   /*  if(user_button == 0) // hilfsstart zustand varbiable muss auf Wert über 10 geändert werden
     {
         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(1900); //1250
       if((30 < ultrarechts.read_cm()) & (ultrarechts.read_cm() < 200))
       {
            //fhelp = ultrarechts.read_cm();
            //printf("%3.3f;",fhelp);
            CZustand = 6;
       }
       else{
            CZustand = 0;
  
       }
        
        
        thread_sleep_for(100);
    }
    
    
    if(CZustand == 3)//links erkennt
    {
       printf("links\n");
       servo_0.SetPosition(900);
       if((30 < ultralinks.read_cm()) & (ultralinks.read_cm() < 200))
       {
            //fhelp = ultralinks.read_cm();
            //thread_sleep_for(10);
            //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(1400);
       if((30 < ultravorne.read_cm()) & (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(2400);
       if((30 < ultrahinten.read_cm()) & (ultrahinten.read_cm() < 200)) 
       {
            CZustand = 6;
        }
        else{
            CZustand = 0;
         
        } 
        
        thread_sleep_for(100);   
    }
    if(CZustand == 6)//spritzen
    {
        printf("spritzen\n");
        summer = 1;
        thread_sleep_for(spritz);
        pumpe = 1;
        thread_sleep_for(spritz); // Spritzzeit
        pumpe = 0;
        summer = 0;
        CZustand = 0;
        servo_0.SetPosition(1400); 
        

    }
 
        /* ------------- 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);
        
    }
  
        
        
        
}