// Horizon artificiel avec le MPU6050 https://forum.arduino.cc/t/horizon-artificiel-avec-le-mpu6050/1207076/3

/*MPU6050 sensor with servo control.
https://srituhobby.com
*/

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

#define VIEWMETER_PIN    5
#define LED_BLINK_RATE 250
#define PAUSE_SERVO   2000


Servo servo;
Adafruit_MPU6050 srituhobby;
uint64_t LedNextBlinkTime;

void setup(void) {
  Serial.begin(115200);
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, LOW);
  servo.attach(3);
  Wire.begin();
  srituhobby.begin();
  servo.write(90);

  srituhobby.setAccelerometerRange(MPU6050_RANGE_8_G);  //2_G,4_G,8_G,16_G
  srituhobby.setGyroRange(MPU6050_RANGE_500_DEG);       //250,500,1000,2000
  srituhobby.setFilterBandwidth(MPU6050_BAND_21_HZ);

  delay(PAUSE_SERVO);
  digitalWrite(LED_BUILTIN, HIGH);
  servo.write(90 + 60);
  delay(PAUSE_SERVO);
  digitalWrite(LED_BUILTIN, LOW);
  servo.write(90);
  delay(PAUSE_SERVO);
  digitalWrite(LED_BUILTIN, HIGH);
  servo.write(90 - 60);
  delay(PAUSE_SERVO);
  digitalWrite(LED_BUILTIN, LOW);
  servo.write(90);
  delay(PAUSE_SERVO);
  LedNextBlinkTime = millis();
  Serial.println("Artificial Horizon Ready");
}

void loop() {

  if (millis() > LedNextBlinkTime)  // Blink the LED
  {
    LedNextBlinkTime += LED_BLINK_RATE;
    digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
  }


  /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  srituhobby.getEvent(&a, &g, &temp);
  int16_t servoValue = (int) (90.0 - 4.5 * a.acceleration.y + 0.5);
  servo.write(servoValue);

  // analogWrite(VIEWMETER_PIN, (int) (128.0 - 6.4 * a.acceleration.y + 0.5);
  uint8_t viewmeterValue = (int) (128.0 - 6.4 * a.acceleration.y + 0.5);
  analogWrite(VIEWMETER_PIN, viewmeterValue);

  Serial.flush();  // Attend que le buffer de sortie série soit vide avant de poursuivre
  Serial.print("Y: ");
  Serial.print(a.acceleration.y);
  Serial.print("  Servo Value ");
  Serial.print(servoValue);

  Serial.print("  Viewmeter Value ");
  Serial.println(viewmeterValue);
  //delay(10);
}