#include <MPU6050.h>
MPU6050 mpu;

int white_pin = 2;
int red_pin = 4;

int gold_pin = 6;
int blue_pin = 8;

void setup() {
  Wire.begin();
  Serial.begin (9600);
  pinMode (white_pin, OUTPUT);
  pinMode (red_pin, OUTPUT);
  pinMode (gold_pin, OUTPUT);
  pinMode (blue_pin, OUTPUT);
  mpu. initialize();


}

void loop() {
  int16_t az = mpu.getAccelerationZ();
  az =constrain(az, -16384, 16384);
  float anglez = az / 16384.0;

  int16_t ax = mpu.getAccelerationX();
  ax = constrain(ax, -16384, 16384);
  float anglex = ax / 16384.0;

  if (anglez < 0) anglez = 90 - degrees(acos (anglez) ) ;
  else anglez = degrees(acos(-anglez)) - 90;

  if (anglex < 0) anglex = 90 - degrees(acos (anglex) ) ;
  else anglex = degrees(acos(-anglex)) - 90;

  Serial.print(anglez);
  Serial.print("---");
  Serial.print(anglex);
  delay(5);

  if(anglez < -15){
    digitalWrite(white_pin, HIGH);
    digitalWrite(red_pin, LOW);
  }else if(anglez > 15){
    digitalWrite(white_pin, LOW);
    digitalWrite(red_pin, HIGH);
  }else{
    digitalWrite(white_pin, LOW);
    digitalWrite(red_pin, LOW);
  }

  if(anglex < -15){
    digitalWrite(gold_pin, HIGH);
    digitalWrite(blue_pin, LOW);
  }else if(anglex > 15){
    digitalWrite(gold_pin, LOW);
    digitalWrite(blue_pin, HIGH);
  }else{
    digitalWrite(gold_pin, LOW);
    digitalWrite(blue_pin, LOW);
  }








}