// РАБОТА С СЕРВОПРИВОДАМИ
//Подключение библиотек
#include <Servo.h>  // библиотека сервоприводов

//Объявление объектов
Servo P_servo;      // сервопривод по тангажу (pitch)
Servo R_servo;      // сервопривод по крену (roll)
Servo Y_servo;      // сервопривод по вращению (yaw)

// Объявление констант и переменных 
#define In_P 2  //назначение пина кнопке "Тангаж"
#define Out_P 8 //назначение пина светодиоду "Тангаж"
#define In_R 3  //назначение пина кнопке "Крен"
#define Out_R 9 //назначение пина светодиоду "Крен"
#define In_Y 4  //назначение пина кнопке "Рыскание"
#define Out_Y 10 //назначение пина светодиоду "Рыскание" 
int T = 500; int A = 5;

void setup() {
  for (int i=2; i<=7; i=i+1) {
    pinMode(i, INPUT_PULLUP);
    pinMode(i+6, OUTPUT);
  }
   Iservo();
}

void loop() {
    if (!digitalRead(In_P)) {
        digitalWrite(Out_P, 0);
        Pservo();
    } else {
        digitalWrite(Out_P, 1);
    }
    if (!digitalRead(In_R)) {
      digitalWrite(Out_R, 0);
      Rservo();
    } else {
      digitalWrite(Out_R, 1);
    }
    if (!digitalRead(In_Y)) {
      digitalWrite(Out_Y, 0);
      Yservo();   
    } else {
      digitalWrite(Out_Y, 1);
    }

} 

//Обяъвление функций
void Iservo() {
   P_servo.attach(11); R_servo.attach(12); Y_servo.attach(13);   
   P_servo.write(90); R_servo.write(90); Y_servo.write(90);
}

void Pservo() {
  for (int pos = 90; pos >= 0; pos -= 1) {
    P_servo.write(pos);
    delay(T);
  }
  for (int pos = 0; pos <= 180; pos += 1) {
    P_servo.write(pos);
    delay(T);
  }
   for (int pos = 180; pos >= 90; pos -= 1) {
    P_servo.write(pos);
    delay(T);
  } 
}

void Rservo() {
  for (int pos = 90; pos >= 0; pos -= 1) {
    R_servo.write(pos);
    delay(T);
  }
  for (int pos = 0; pos <= 180; pos += 1) {
    R_servo.write(pos);
    delay(T);
  }
   for (int pos = 180; pos >= 90; pos -= 1) {
    R_servo.write(pos);
    delay(T);
  } 
}

void Yservo() {
  for (int pos = 90; pos >= 0; pos -= 1) {
    Y_servo.write(pos);
    delay(T);
  }
  for (int pos = 0; pos <= 180; pos += 1) {
    Y_servo.write(pos);
    delay(T);
  }
   for (int pos = 180; pos >= 90; pos -= 1) {
    Y_servo.write(pos);
    delay(T);
  } 
}