#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

float angle_tilt;

Adafruit_MPU6050 mpu;

void setup(void) {

  pinMode(10, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(4, OUTPUT);

  Serial.begin(115200);

  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }

  // mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
  // mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  // mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  // Serial.println("");
  delay(100);
}

void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  angle_tilt = g.gyro.x; 


  Serial.print(g.gyro.x);
  Serial.print(",");
  Serial.print(g.gyro.y);
  Serial.print(",");
  Serial.print(g.gyro.z);
  Serial.print(", ");
  Serial.print(angle_tilt);
  Serial.println("");

  if ((angle_tilt < -1.75) && (angle_tilt > -4.36))
  {
    digitalWrite(10, HIGH);
    digitalWrite(9,LOW);
    digitalWrite(4,LOW);
  }
  else if((angle_tilt < 1.75) && (angle_tilt > -1.75))
  {
    digitalWrite(9,HIGH);
    digitalWrite(4,LOW);
    digitalWrite(10,LOW);
  }
  else if((angle_tilt < 4.36) && (angle_tilt > 1.75))
  {
    digitalWrite(4,HIGH);
    digitalWrite(9,LOW);
    digitalWrite(10,LOW);
  }else
  {
    digitalWrite(4,LOW);
    digitalWrite(9,LOW);
    digitalWrite(10,LOW);
  }

  delay(100);
}