/*Dieser Sketch laesst das Auto vorwaerts fahren. Der Fahrablauf wird festgelegt, indem die Fahrablauf_Module aufgerufen werden.
  Der jeweilige Fahrzustand wird im Serial Monitor angezeigt.
  Das Fahren vorwaerts wird gestoppt, wenn ein Hindernis auftaucht mit Abstand kleiner 25cm. Dann ertoent auch der Buzzer (Piezo-Summer).
  Die Entfernung wird mit einem Ultraschallsensor HC-SR04 gemessen und auch angezeigt im Serial Monitor.
  Es ist ein Passive Buzzer, man kann unterschiedliche Tonsignal-Frequenzen programmieren*/

/*Binde den Programmablauf von Fahrablauf_Module.h ein*/
#include "Fahrablaufmodule.h"
/*Binde den Programmablauf von us_hindernisabfrage.h ein*/
  #include "us_hindernisabfrage.h"
  
  void setup()
  {
  /*Programmiere Arduino Pins als Ausgang (fuer Motoren)*/  
    pinMode(6, OUTPUT);                   //Motoren LINKS PWM (Geschwindigkeit)
    pinMode(5, OUTPUT);                   //Motoren RECHTS PWM (Geschwindigkeit)
    pinMode(11, OUTPUT);                  //Motoren LINKS Brücke
    pinMode(8, OUTPUT);                   //Motoren LINKS Brücke
    pinMode(7, OUTPUT);                   //Motoren RECHTS Brücke
    pinMode(17, OUTPUT);                  //Motoren RECHTS Brücke
    /*Programmiere Arduino Pins D2,D4 und A5=D19 als Ausgang bzw Eingang(fuer HC-SR04*/  
    pinMode(2, OUTPUT);                   //Signal Trig zum HC-SR04
    pinMode(19, INPUT);                   //Signal Echo vom HC-SR04, Pin A5=D19
    pinMode(4, OUTPUT);                   //Tonsignal zum Buzzer  
  /*Serial Monitor aktivieren, 9600 Baud*/
    Serial.begin(9600); 
  
    delay(500);                           //Warte 0,5 Sekunde bis das Auto losfaehrt
    forward();                            //void forward wird aufgerufen
  }
  
  void loop()
  {
   us_hindernisabfrage();                //us_hindernisabfrage wird aufgerufen 
   delay(200);                           //Warte 0,2 Sekunde bis erneut abgefragt wird           
  }

  void forward()
  {   
    digitalWrite(7, HIGH);            // Schalte Motoren RECHTS ein 
    digitalWrite(17, LOW);                                                      
    analogWrite(5, 120);              // Setze die Geschwindigkeit auf 120 (zwischen 0...255)
    digitalWrite(11, HIGH);           // Schalte Motoren LINKS ein 
    digitalWrite(8, LOW);                                
    analogWrite(6, 120);              // Setze die Geschwindigkeit auf 120 (zwischen 0...255)
    Serial.println("FORWARD");
  }
  
  void back()
  {   
    digitalWrite(7, LOW);            // Schalte Motoren RECHTS ein 
    digitalWrite(17, HIGH);                                                      
    analogWrite(5, 120);             // Setze die Geschwindigkeit auf 120 (zwischen 0...255)
    digitalWrite(11, LOW);           // Schalte Motoren LINKS ein 
    digitalWrite(8, HIGH);                                
    analogWrite(6, 120);             // Setze die Geschwindigkeit auf 120 (zwischen 0...255)
    Serial.println("BACK");
  }
  
  void left()
  {   
    digitalWrite(7, HIGH);           // Schalte Motoren RECHTS ein 
    digitalWrite(17, LOW);                                                      
    analogWrite(5, 120);             // Setze die Geschwindigkeit auf 120 (zwischen 0...255)
    digitalWrite(11, LOW);           // Schalte Motoren LINKS ein 
    digitalWrite(8, HIGH);                                
    analogWrite(6, 120);             // Setze die Geschwindigkeit auf 120 (zwischen 0...255)
    Serial.println("LEFT");
  }
  
  void right()
  {   
    digitalWrite(7, LOW);            // Schalte Motoren RECHTS ein 
    digitalWrite(17, HIGH);                                                      
    analogWrite(5, 120);             // Setze die Geschwindigkeit auf 120 (zwischen 0...255)
    digitalWrite(11, HIGH);          // Schalte Motoren LINKS ein 
    digitalWrite(8, LOW);                                
    analogWrite(6, 120);             // Setze die Geschwindigkeit auf 120 (zwischen 0...255)
    Serial.println("RIGHT");
  }
  
  void stopp()
  {
    digitalWrite(7, LOW);            // Schalte alle Motoren aus
    digitalWrite(17, LOW);  
    digitalWrite(11, LOW); 
    digitalWrite(8, LOW);  
    Serial.println("STOP");  
  }
  
  

  /*Zunaechst werden einige Variablen definiert*/
    long dauer=0;                       // in der Variablen "dauer" soll die Zeit gespeichert werden, die eine Schallwelle bis zur Reflektion und zurueck benoetigt. 
    long entfernung=0;                  // in der Variablen "entfernung" soll die berechnete Entfernung gespeichert werden. 
  
  
  void us_hindernisabfrage()
  {
  /*Dieser Programmteil ermittelt die Entferung zu einem Hindernis und stoppt das Auto sowie aktiviert den Buzzer gegebenenfalls*/
    digitalWrite(2, LOW);               //Hier nimmt man die Spannung fuer kurze Zeit vom Trig-Pin, fuer ein rauschfreies Signal beim Senden
    delay(5);                           //...fuer die Dauer 5 Millisekunden
    digitalWrite(2, HIGH);              //Ein HIGH-Signal wird zum Trig-Eingang des HC-SR04 gesendet
    delayMicroseconds(15);              //Die Dauer muss mindestens 10 Mikrosekunden sein, hier gewaehlt 15
    digitalWrite(2, LOW);               //Wenn das Trig-Signal wieder auf LOW geht, sendet der HC-SR04 Ultraschallwellen aus (8mal, Frequenz 40kHz)
    dauer = pulseIn(19, HIGH);          //Der Arduino zaehlt die Zeit in Mikrosekunden, bis der reflektierte Schall zum Ultraschallsensor zurueckkehrt
    entfernung = (dauer/2) * 0.03432;   //Nun wird aus der Zeit die Entfernung in Zentimetern berechnet (Bsp.: 10ms entspricht Entfernung 170cm)
    Serial.print(entfernung);           //Der Wert der Entfernung wird an den Serial Monitor uebergeben
    Serial.println(" cm");              //Hinter dem Wert der Entfernung soll die Einheit "cm" angegeben sowie die Anzeigezeile gewechselt werden (CR)
    if (entfernung < 25)                //Wenn der Wert fuer die Entfernung unter 25 Zentimeter betraegt dann... 
    {
    tone(4,1000,1000);                  //...Sende Signal zum Buzzer (Arduino erzeugt an D4 einen Ton 1000Hz fuer Dauer 1000ms)
    digitalWrite(9, LOW);               //Schalte alle Motoren aus   
    digitalWrite(8, LOW);  
    digitalWrite(7, LOW); 
    digitalWrite(6, LOW);  
    for (;;) {}                         //Beende Sketch
    }
    else                                //Und wenn das nicht so ist...
    {
    digitalWrite (4, LOW);              //Tue nichts, gehe zurueck zu void loop
    }
  }