//Motor ansteuern

//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      11     //Rechten Motor an PWM-Eingang 9
#define M_links       12    //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 = 100; //Max PWM, Motor dauernd an...
int Max_links = 100; //... 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 = B00010010; //ICNC1 ICES1 – WGM13 WGM12 CS12 CS11 CS10
  ICR1 = 255; //Top value
  OCR1A = 255; //Output Compare Register
  OCR1B = 255; //Output Compare Register
}

//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(); //Aufruf der Funktion "vorwaerts", ohne Parameterübergabe
}

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

/*Aufgabe: Funktionen für rechts, links und stopp implementieren
Rückwärts fahren, im Kreis drehen, links rum, rechts rum, Anhalten*/

/*Auszug aus dem Manual vom Motortreiber DRV8835:
Drive/brake operation in default PHASE/ENABLE mode
xPHASE  xENABLE   MxA   MxB   operating mode
0       PWM       PWM   L     forward/brake at speed PWM %
1       PWM       L     PWM   reverse/brake at speed PWM %
X       0         L     L     brake low (outputs shorted to ground)*/