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

#define PIN_TRIG 2
#define PIN_ECHO 3

//TRIMMER MANSA
  int trimmer_tangaj_fata = 0;
  int trimmer_ruliu_dreapta = 0;
  int trimmer_tangaj_spate = 0;
  int trimmer_ruliu_stanga = 0;
  int ap_overright = 0;


//IRS
int altimetru = 0;
int viteza = 0;
int unghi_de_atac = 0;
int frana_aero_scos = 0;
int frana_aero_bagat = 0;
Servo myservo;  // create servo object to control a servo
// twelve servo objects can be created on most boards

int pos = 0;    // variable to store the servo position
void setup() {

  Serial.begin(115200);

  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  Serial.println("MPU6050 ready!");
   myservo.attach(11);
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);
  pinMode(12, INPUT);
  pinMode(13, INPUT);
  pinMode(6, INPUT);
  pinMode(7, INPUT);
  pinMode(8, INPUT);
  pinMode(9, INPUT);
  pinMode(10, INPUT);
}
sensors_event_t event;
void loop() {

frana_aero_scos = digitalRead(12);
frana_aero_bagat = digitalRead(13);
trimmer_ruliu_stanga = digitalRead(6);
trimmer_ruliu_dreapta = digitalRead(7);
trimmer_tangaj_fata = digitalRead(8);
trimmer_tangaj_spate = digitalRead(9);
ap_overright = digitalRead(10);

  // Start a new measurement:
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);

  // Read the result:
  viteza = map(analogRead(28), 0, 1023, 0, 1000);
  int duration = pulseIn(PIN_ECHO, HIGH);
altimetru = 0.01723 * (duration / 58);
unghi_de_atac = analogRead(26);

if (frana_aero_scos == HIGH){
 for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  }
}
if (frana_aero_bagat == HIGH){
  for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
    myservo.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  }
}
  mpu.getAccelerometerSensor()->getEvent(&event);

  Serial.print("[");
  Serial.print(millis());
  Serial.print("] X: ");
  Serial.print(event.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(event.acceleration.y);
  Serial.print(", Z: ");
  Serial.print(event.acceleration.z);
  Serial.println(" m/s^2");
  delay(500);
}