//Funktionen

//Definitionen
#define LED_Pin       13    //LED an Pin 13 betreiben
#define M_rechts_DIR  7     //rechten Motor an Pin 7 vor- oder rückwärts schalten
#define M_links_DIR   8     //linken Motor an Pin 8 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, Register für Timer/Compare
#define M_links_PWM   OCR1B //PWM-Signal Duty im Verhältnis zu ICR1, Register für Timer/Compare

//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(M_rechts_DIR, OUTPUT); //Pin 7 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
}

//Deklarationen
void vorwaerts (int);
void rueckwarts (int);
void rechts (int);
void links (int);
void stopp (void);

//Hauptprogramm
void loop() {
  //Arbeitsschleife
  //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
//  }
  vorwaerts(200); //Aufruf der Funktion "vorwaerts", mit Parameterübergabe
  //Aufgabe: Quadrat abfahren
}

//Vier Bits beeinflussen die Richtung und die Geschwindigkeit der Motoren
//void -> nichts wird zurückgegeben, vorwaerts -> Name der Funktion, (void) -> es wurden KEINE Parameter bei Aufruf der Funktion mit übergeben
void vorwaerts (int vel) {
  digitalWrite(M_rechts_DIR, HIGH); //Beide Motoren...
  digitalWrite(M_links_DIR, HIGH); //...volle Pulle...
  M_rechts_PWM = vel; //...vorwärts...
  M_links_PWM = vel;  //...drehen lassen
}
void rueckwaerts (int vel) {
  digitalWrite(M_rechts_DIR, LOW); //Beide Motoren...
  digitalWrite(M_links_DIR, LOW); //...volle Pulle...
  M_rechts_PWM = vel; //...rückwärts...
  M_links_PWM = vel;  //...drehen lassen
}
void rechts (int vel) {
  digitalWrite(M_rechts_DIR, LOW); //
  digitalWrite(M_links_DIR, HIGH); //
  M_rechts_PWM = vel; //
  M_links_PWM = vel;  //
}
void links (int vel) {
  digitalWrite(M_rechts_DIR, HIGH); //
  digitalWrite(M_links_DIR, LOW); //
  M_rechts_PWM = vel; //
  M_links_PWM = vel;  //
}
void stopp (void) {
  digitalWrite(M_rechts_DIR, HIGH); //Beide Motoren...
  digitalWrite(M_links_DIR, HIGH); //...nicht...
  M_rechts_PWM = 0; //...
  M_links_PWM = 0;  //...drehen lassen
}