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

// 舵机引脚
uint8_t servoPin = 6;
// 陀螺仪选择的轴 0:x  1:y  2:z
uint8_t axisIndex = 1;
// 初始角度（x/y/z）的一个方向
float initAngle;
// 舵机初始位置
float servoInitPos = 90;

Servo myservo;
Adafruit_MPU6050 mpu;

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

  myservo.attach(servoPin);
  myservo.write(servoInitPos);

  // Try to initialize!
  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);

  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  switch (axisIndex){
    case 0:
      initAngle = g.gyro.x;
      break;
    case 1:
      initAngle = g.gyro.y;
      break;
    case 2:
      initAngle = g.gyro.z;
      break;
  }
}

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

  float currentAngle;
  switch (axisIndex){
    case 0:
      currentAngle = g.gyro.x;
      break;
    case 1:
      currentAngle = g.gyro.y;
      break;
    case 2:
      currentAngle = g.gyro.z;
      break;
  }
  Serial.print("currentAngle:" ); Serial.println(currentAngle);
  float stepAngle = (currentAngle - initAngle) * 180 / 3.1415964;
  Serial.print("stepAngle:" ); Serial.println(stepAngle);

  float servoAngle = servoInitPos - stepAngle;    // + or -
  if((servoAngle) < 0){
    myservo.write(0);
  }else if((servoAngle) > 180){
    myservo.write(180);
  }else{
    myservo.write(servoAngle);
  }

  // 加速度
  // a.acceleration.x

  // 角度
  // g.gyro.x
  // *180/3.1415964 將角度radian(弧度） 轉為 度
  // Serial.print(a.acceleration.x);
  // Serial.print(",");
  // Serial.print(a.acceleration.y);
  // Serial.print(",");
  // Serial.print(a.acceleration.z);
  // Serial.print(", ");
  // Serial.print(g.gyro.x);
  // Serial.print(",");
  // Serial.print(g.gyro.y);
  // Serial.print(",");
  // Serial.print(g.gyro.z);
  // Serial.println("");

  delay(10);

}