#include<Wire.h>

const int MPU_addr=0x68;
int16_t axis_X,axis_Y,axis_Z;
int minVal=265;
int maxVal=402;
double x;
double y;
double z;

const int DIR1 = 3;
const int STEP1 = 5;
const int DIR2 = 4;
const int STEP2 = 6;

const int maxDeviation = 5;
const int idealPosition = 180;

void setup()
{
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);

  Serial.begin(9600);

  pinMode(DIR1, OUTPUT);
  pinMode(STEP1, OUTPUT);
  pinMode(DIR2, OUTPUT);
  pinMode(STEP2, OUTPUT);

}
void loop()
{
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);

  axis_X = Wire.read()<<8|Wire.read();
  axis_Y = Wire.read()<<8|Wire.read();
  axis_Z = Wire.read()<<8|Wire.read();

  int xAng = map(axis_X,minVal,maxVal,-90,90);
  int yAng = map(axis_Y,minVal,maxVal,-90,90);
  int zAng = map(axis_Z,minVal,maxVal,-90,90);

  z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);

  Serial.println(z);

  int down = idealPosition - maxDeviation;
  int up = idealPosition + maxDeviation;
  
  if((z > up) || (z < down)) 
  {
    if(z > up) 
    {
      digitalWrite(DIR1, LOW);
      digitalWrite(DIR2, HIGH);
    }

    else if(z < down)
    {
      digitalWrite(DIR1, HIGH);
      digitalWrite(DIR2, LOW);
    }
      analogWrite(STEP1, 120);
      analogWrite(STEP2, 120);
      //Serial.println("ok");
      delay(100); 
  }
}