/* aufgabeL1-3.ino
 *  Handreichungen Wahlfach BPE 10 Robotik
*/ 

//  Übersicht über die Ansteuerung des L298n Moduls für die Fahrzeugbewegung
//  ENA   ENB   IN1   IN2   IN3   IN4   Pins
//  HIGH  HIGH  HIGH  LOW   LOW   HIGH  Auto fährt vorwärts
//  HIGH  HIGH  LOW   HIGH  HIGH  LOW   Auto fährt rückwärts
//  HIGH  HIGH  LOW   HIGH  LOW   HIGH  Auto dreht nach rechts
//  HIGH  HIGH  HIGH  LOW   HIGH  LOW   Auto dreht nach link
//  HIGH  HIGH  LOW   LOW   LOW   LOW   Auto hält an
//  HIGH  HIGH  HIGH  HIGH  HIGH  HIGH  Auto hält an
//  LOW   LOW   N/A   N/A   N/A   N/A   Auto hält an


// L298n Modul IO Pins für den gewählten Fahrroboter
#define ENA 5
#define ENB 6
#define IN1 7
#define IN2 8
#define IN3 9
#define IN4 11

typedef unsigned char u8;


long myTime = 0;

// Richtungen einstellen
void forward() {
  digitalWrite(IN1, HIGH); //setze IN1 auf High
  digitalWrite(IN2, LOW);  //setze IN2 auf Low
  digitalWrite(IN3, LOW);  //setze IN3 auf Low
  digitalWrite(IN4, HIGH); //setze IN4 auf Hight
  //  Serial.println("Forward");
}

void back() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  //  Serial.println("Back");
}

void left() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  //  Serial.println("Left");
}

void right() {
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  //  Serial.println("Right");
}

void stop() {
  digitalWrite(ENA, LOW);
  digitalWrite(ENB, LOW);
  //  Serial.println("Stop");
}


// Geschwindigkeit setzen
void setSpeed(u8 speed) {
  analogWrite(ENA, speed);
  analogWrite(ENB, speed);
}


//before execute loop() function,
//setup() function will execute first and only execute once
void setup() {
  Serial.begin(9600);//open serial and set the baudrate
  pinMode(IN1, OUTPUT); //before useing io pin, pin mode must be set first
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);
}

//Repeat execution
void loop() {
  forward();  //go forward
  setSpeed(100);
  delay(1000);
  right();
  setSpeed(120);
  delay(2000);
  stop();
  exit(0);
}
