/* aufgabeL2-2.ino
    Handreichungen Wahlfach BPE 10 Robotik
*/


#include "robotlib.h"
#include "TimerOne.h"

int distance = 1000;
u8 speed = 180;
boolean initialized = false;
boolean turn = false;
int steps = 0;

//before execute loop() function,
//setup() function will execute first and only execute once
void setup() {
  Serial.begin(9600);//open serial and set the baudrate
  initMotor();
  initSonicSensor();

  Timer1.initialize(100000); // auf 0,1 Sekunden eingestellt
  // hier geben wir den Namen der Funktion an, die aufgerufen wird
  // wenn der Timer abgelaufen ist, also jeweils nach 0,1s
  Timer1.attachInterrupt(timer_isr);
}


void turnAround() {
  right();
  setSpeed(150);
}

//Repeat execution
// durch die Interrupt-Funktion müssen wir uns nicht
// um die Regelmäßigkeit der Messungen kümmern
void loop() {
  while (!initialized) return;
  //Serial.println(steps);
  if (turn) {
    turnAround();
  } else {
    forward();
    setSpeed(speed);
    // Calculating the distance
    if (distance <= 20) {
      speed = speed / 2;
      if (distance < 10) {
        speed = 0;
        turn = true;
      }
    }
  }
}

// Diese Funktion wird aufgerufen nach 0,1 s
void timer_isr(void) {
  initialized = true;
  distance = getDistanceFromSonic();
  if (turn) {
    steps++;
    if (steps > 15) {
      stop();
      exit(0);
    }
  }
}
