// aufgabeL3-1.ino

#include <Wire.h>
#include <MPU6050_light.h>
#include "robotlib.h"

MPU6050 Mpu(Wire);

int distance;
float XAngle, YAngle,ZAngle;


void initGyro() {
  Wire.begin();
  Mpu.begin();
  // Achtung: 1 Sekunde nicht bewegen zur Kalibration des Offset
  delay(1000);
  Mpu.calcOffsets();
}

//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();
  initGyro();
}

//Repeat execution
void loop() {
  distance = getDistanceFromSonic();
  distance = getDistanceFromSonic();
  Mpu.update();

  XAngle = Mpu.getAngleX();
  YAngle = Mpu.getAngleY();
  ZAngle = Mpu.getAngleZ();
}
