//Monitor, if-Anweisung
//Definitionen
#define LED_Pin       13    //LED an Pin 13 betreiben#define M_rechts      9     //Rechten Motor mit Pin 9 ein- oder ausschalten
#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 an PWM-Eingang 9
#define M_links       10    //Linken Motor an PWM-Eingang 10
#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 A3  //Rechter Liniensensor an Analogeingang 3
#define mitte_LinSe A4  //Mittlerer Liniensensor an Analogeingang 4
#define linker_LinSe  A5  //Linker Liniensensor an Analogeingang 5
uint8_t Schwellwert_rechts = 150;
#define rechts_dunkel analogRead(rechter_LinSe) < 150 //anzupassender Wert, rechter Liniensensor
#define mitte_dunkel analogRead(mitte_LinSe) < 150 //anzupassender Wert, mittlerer Liniensensor
#define links_dunkel  analogRead(linker_LinSe) < 150 //anzupassender Wert, linker Liniensensor

//Variablendeklaration
int Max_rechts = 400; //Max PWM, Motor dauernd an...
int Max_links = 400; //... hier kann die Grundgeschwindigkeit eingestellt werden
int Time; //Zähler für Blinken auf Pin 13
unsigned long Time_millis_old; //32 Bit = 2^32 - 1 = 4294967295

//Initialisierung (Einstellungen am MikroController)
void setup() {
  pinMode(LED_Pin, OUTPUT); //Pin 13 als Ausgang für LED festlegen
  pinMode(rechter_LinSe, INPUT); //Analog Ein/Ausgang A3 als Eingang konfigurieren
  pinMode(mitte_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); //Pin 6 als Ausgang festlegen
  pinMode(M_links_DIR, OUTPUT); //Pin 8 als Ausgang festlegen
  pinMode(M_rechts, OUTPUT); //PWM-Pin als Ausgang
  pinMode(M_links, OUTPUT); // dito
  // PWM Frequenz einstellen
  // 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
  
  Serial.begin(9600); //Baud Rate für serielle Übertrageung 9600 Bit/sec
}

//Hauptprogramm
void loop() {
  //Arbeitsschleife
  //Werte über Monitor auslesen, im setup() letzte Zeile unkommentieren
  while(1) { //Testschleife  
    Serial.println(analogRead(rechter_LinSe));
    Serial.println(analogRead(mitte_LinSe));
    Serial.println(analogRead(linker_LinSe));
          Serial.println("\n"); //neue Zeile
  }
  //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
  }
  //Blinklicht an Pin 13, anhand der Anzahl, der Durchläufe des Programms
//  Time++; //Time um 1 erhöhen, inkrementieren
//  if(Time == 800) { //nach 800 Programmdurchläufen...
//    PORTB ^= B00100000; //...wird Pin 13 getoggelt
//    Time = 0; //Zähler wieder auf 0
//  }

  //Linie folgen
      if (rechts_dunkel) { //rechter Liniensensor ist dunkel
        if (links_dunkel) { //linker Liniensensor ist dunkel (beide dunkel, Kreuzung)
          vorwaerts(); //Beide Motoren volle Pulle vorwärts drehen lassen
        }
        else { //linker Liniensensor ist hell (rechter ist dunkel)
          rechts(); //rechter Motor dreht links oder aus, linker rechts, volle Pulle
        }
      }
      else { //rechter Liniensensor ist hell
        if (links_dunkel) { //linker Liniensensor ist dunkel
          links(); //rechter Motor dreht rechts , linker links oder aus, volle Pulle
        }
        else { //beide Liniensensoren sind hell (Lücke)
          stopp(); //Beide Motoren volle Pulle vorwärts drehen lassen
        }
      }
}

//Vier Bits beeinflussen die Richtung und die Geschwindigkeit der Motoren
void vorwaerts (void) {
  digitalWrite(M_rechts_DIR, HIGH); //Beide Motoren...
  digitalWrite(M_links_DIR, HIGH); //...volle Pulle...
  M_rechts_PWM = Max_rechts; //...vorwärts...
  M_links_PWM = Max_links;  //...drehen lassen
}

void rechts (void) {
  digitalWrite(M_rechts_DIR, LOW); //Rechter Motor rückwaerts
  digitalWrite(M_links_DIR, HIGH); //Linker Motor vorwärts
  M_rechts_PWM = Max_rechts; //Beide volle...
  //M_rechts_PWM = 0; //oder aus
  M_links_PWM = Max_links; //...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; //Beide volle...
  M_links_PWM = Max_links; //...Pulle
  //M_links_PWM = 0; //oder aus
}

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