//Definitionen
#define LED_Pin       13    //LED an Pin 13 betreiben
#define M_rechts_DIR  7     //rechten Motor an Pin 6 vor- oder rückwärts schalten
#define M_links_DIR   8     //linken Motor an Pin 7 vor- oder rückwärts schalten
#define M_rechts      9     //Rechten Motor mit Pin 9 ein- oder ausschalten
#define M_links       10    //Linken Motor mit Pin 10 ein- oder ausschalten
#define M_rechts_PWM  OCR1A //PWM-Signal Duty im Verhältnis zu ICR1
#define M_links_PWM   OCR1B //PWM-Signal Duty im Verhältnis zu ICR1
#define rechter_LinSe A4    //Rechter Liniensensor an Analogeingang 4
#define linker_LinSe  A5    //Linker Liniensensor an Analogeingang 5
#define rechts_dunkel analogRead(rechter_LinSe) < 150   //anzupassender Wert, rechter Liniensensor
#define links_dunkel  analogRead(linker_LinSe) < 150    //anzupassender Wert, linker Liniensensor
#define Trigger_US     4     //Ultraschalltriggerpin
#define Echo           2     //Ultraschallechopin

//Variablendeklaration
int Max_rechts = 400; //Max PWM, Motor...
int Max_links = 400; //...dauernd an
int Time; //Zähler für Blinken auf Pin 13
unsigned long Time_millis_old;
unsigned long US_Takt; //alle 40 Millisekunden US neu initialisieren
volatile unsigned long Echo_Time; //Laufzeit des Echos
unsigned long Echo_Time_begin; //Anfang der US-Messung
unsigned long Echo_Time_Temp; //Für die Verarbeitung des Signals

//Initialisierung (Einstellungen am MC)
void setup() {
  pinMode(LED_Pin, OUTPUT); //Pin 13 als Ausgang für LED festlegen
  pinMode(rechter_LinSe, INPUT); //Analog Ein/Ausgang A4 als Eingang konfigurieren
  pinMode(linker_LinSe, INPUT); //Analog Ein/Ausgang A5 als Eingang konfigurieren
  pinMode(M_rechts_DIR, OUTPUT); //PWM-Pin als Ausgang
  pinMode(M_links_DIR, OUTPUT); //dito
  pinMode(M_rechts, OUTPUT); //Pin 9 als Ausgang festlegen
  pinMode(M_links, OUTPUT); //Pin 10 als Ausgang festlegen
  pinMode(Echo, INPUT); //Pin 2 als Eingang festlegen
  pinMode(Trigger_US, OUTPUT); //Pin 4 als Eingang festlegen
  //
  // PWM frequency calculation
  // 16MHz / 1 (prescaler) / 2 (phase-correct) / 400 (top) = 20kHz
  //TCCR1A: [WGM13..10 (1000)]
    //  1010  Clear OC1A/OC1B on Compare Match when upcounting. Set OC1A/OC1B on Compare Match when downcounting.
    //  xx    not implemented
    //  00    s. TCCR1B
  TCCR1A = B10100000; //COM1A1 COM1A0 COM1B1 COM1B0 – – WGM11 WGM10
  //TCCR1B: [WGM13..10 (1000)]
    //  0     Noise Canceler disabled
    //  0     Input Capture Edge Select disabled (ICR1 is used as TOP value)
    //  x     not implemented
    //  10    PWM, Phase and Frequency Correct,ICR1 = TOP, Update OCR1x = BOTTOM, TOV1-Flag = BOTTOM
    //  001   clkI/O/1 (No prescaling)
  TCCR1B = B00010001; //ICNC1 ICES1 – WGM13 WGM12 CS12 CS11 CS10
  ICR1 = 400; //Top value
  OCR1A = 400; //Output Compare Register
  OCR1B = 400; //Output Compare Register
  //
  M_rechts_PWM = 0; //Erstmal die Räder...
  M_links_PWM = 0; //...still halten
  attachInterrupt(digitalPinToInterrupt(Echo), US, FALLING); //Interrupt durch fallende Flanke an Echo-Pin ruft Funktion "US" auf
  Serial.begin(9600); //Baud Rate für serielle Übertrageung 9600 Bit/sec
}

//Hauptprogramm (Ein Durchlauf dauert ca. 500us)
void loop() {
//  while(1) {
//    vorwaerts();
//  }
  //Blinklicht an Pin 13, alle 200ms, mit millis()-Funktion
  if(millis()- Time_millis_old > 200) { //Vergangene Zeit, seit dem letzten Toggeln
    digitalWrite(LED_Pin,!digitalRead(LED_Pin)); //LED toggeln
    Time_millis_old = millis(); //neue Zeit, die später die alte zeit ist
  }
  //US initialisieren, alle 40 ms
  if(millis()- US_Takt >= 40) {
    Echo_Time_Temp = Echo_Time - Echo_Time_begin; //Berechnung des aktuellen Abstands
    digitalWrite (Trigger_US, HIGH); //Pin 4 auf 5V setzen
      delayMicroseconds(10); //10 Mikrosekunden warten
    digitalWrite (Trigger_US, LOW); //Pin 4 wieder auf GND setzen
    Echo_Time_begin = micros(); //Start der Zeitmessung des US-Signals
    US_Takt = millis(); //Anfang der 40 ms
  }
  //Auswertung US-Signal
  //334 m/s Schallgeschwindigkeit = 30us/cm
  //15cm = 900us, 460us für die Initialisierung
  if(Echo_Time_Temp < 1360) { //Hindernis im Weg
    stopp();
  }
  else { //kein Hindernis
    links();
  }
}


void rechts (void) {
  digitalWrite(M_rechts_DIR, LOW); //Rechter Motor rückwaerts
  digitalWrite(M_links_DIR, HIGH); //Linker Motor vorwärts
  M_rechts_PWM = 0; //Aus oder Max_rechts oder 1..399
  M_links_PWM = Max_links; //Volle Pulle
}

void links (void) {
  digitalWrite(M_rechts_DIR, HIGH); //Rechter Motor vorwärts
  digitalWrite(M_links_DIR, LOW);  //Linker Motor rückwaerts
  M_rechts_PWM = Max_rechts; //Aus oder Max_links oder 1..399
  M_links_PWM = 0;//Max_links; //Volle Pulle
}

void stopp (void) {
  digitalWrite(M_rechts_DIR, HIGH); //Rechter Motor vorwärts
  digitalWrite(M_links_DIR, HIGH);  //Linker Motor vorwärts
  M_rechts_PWM = 0; //Beide Motoren stopp
  M_links_PWM = 0; //
}


//Interrupt: Fallende Flanke an Pin 2
void US (void) {
  Echo_Time = micros(); //aktuelle Zeit in us
  Serial.println(Echo_Time - Echo_Time_begin);
}