/*  MPU6050 Triple Axis Gyroscope & Accelerometer. Pitch & Roll & Yaw Gyroscope Example.
  Read more: http://www.jarzebski.pl/arduino/czujniki-i-sensory/3-osiowy-zyroskop-i-akcelerometr-mpu6050.html
  GIT: https://github.com/jarzebski/Arduino-MPU6050
  Web: http://www.jarzebski.pl
  (c) 2014 by Korneliusz Jarzebski
*/

#include <Wire.h>
#include "MPU6050.h"

MPU6050 mpu;
const byte PIN_BTN0 = A0;

void setup()
{
  Serial.begin(115200);

  // Initialize MPU6050
  while (!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
  {
    Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
    delay(500);
  }
  pinMode (PIN_BTN0, INPUT_PULLUP);

  // Calibrate gyroscope. The calibration must be at rest.
  // If you don't want calibrate, comment this line.
  mpu.calibrateGyro();

  // Set threshold sensivty. Default 3.
  // If you don't want use threshold, comment this line or set 0.
  mpu.setThreshold(3);
}

bool enable = false;
void loop() {
  bool res = digitalRead(PIN_BTN0);
  if (res == LOW) {
    delay(50);
    while ( !digitalRead(PIN_BTN0) );
    //ServoZ.write(0);.
    Serial.println("ServoZ.write(0)");
    while (true);  // resta qui in un ciclo infinito
  }

  // Read normalized values
  Vector norm = mpu.readNormalizeGyro();

  Serial.print("ZAxis = ");
  Serial.println(norm.ZAxis);
  if (enable == true &&  norm.ZAxis > 220) {
    //ServoZ.write(180);
    Serial.println("Servo(180)");
    Serial.println("delay(500)");
    delay(500);    // 500ms nella speranza che il servo non ci impieghi più tempo.
    //ServoZ.write(0);
    Serial.println("Servo(0)");
    Serial.println("delay(500)");

    delay(500);
    enable = false;  // da questo momento enable == false.
  } else if ( norm.ZAxis < 5)  {
    // altrimenti se  norm.ZAxis < 5 gradi enable = true.
    enable = true;
  }

  delay(200);
}