#include "robotlib.h"

void initMotor() {
  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);
}

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

void back() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}

void left() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}

void right() {
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}

void stop() {
  digitalWrite(ENA, LOW);
  digitalWrite(ENB, LOW);
}


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

void initSonicSensor() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}


int getDistanceFromSonic() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  long duration = pulseIn(echoPin, HIGH);
  //  Serial.print("Signaldauer: ");
  //  Serial.println(duration);
  // Calculating the distance
  return duration / 58; // Speed of sound wave divided by 2 (go and back)
}




