// Подключаем библиотеки
#include <Servo.h>

// Определяем пины
#define SERVO_1 3
#define SERVO_2 5
#define BUTTON 13 

// Выставляем позиции
const uint8_t start_angle = 0;
const uint8_t end_angle = 180;

// Создаем объекты класса
Servo servo_1;
Servo servo_2;

// Выключаем нашу кнопку
int oldValue = LOW; 

// Подключаем наши пины
void setup() {
  Serial.begin(9600);
  servo_1.attach(SERVO_1);
  servo_2.attach(SERVO_2);
  pinMode(BUTTON, INPUT);
  // put your setup code here, to run once:
}


void loop()
{
  // Проверяем нажата ли кнопка, если нажата, то запускаем вращение серв
  if(buttom_control())
  {
    servo_working( &servo_1, &servo_2, start_angle, end_angle);
    servo_working( &servo_1, &servo_2, end_angle, start_angle);
  }

}

// Функция принимающая объекты класса, стартовое и конечное положение
void servo_working(Servo *ser_1, Servo *ser_2, uint8_t PtStart, uint8_t PtEnd)
{
  // Сравнение положения для того, что бы понять в какую сторону идти
  if(PtStart > PtEnd)
  {
    //Цикл поворота
    for(uint8_t i = PtStart; i > PtEnd; i--)
    {
      // Проверка на выключение, если выключили принудительно останавливаем
      if(!buttom_control())
      {
        break;
      }
      //Команда на управление сервой
      ser_1->write(i);
      //Команда на управление сервой в обратную сторону
      ser_2->write(PtStart-i);
      Serial.print("1 ");
      Serial.println(i);
      delay(15);
    }
  }
  else
  // описывает тот же самый процесс обратного условия
  {
    for(uint8_t i = PtStart; i < PtEnd; i++)
    {
      if(!buttom_control())
      {
        break;
      }
      ser_1->write(i);
      ser_2->write(PtEnd-i);
      Serial.print("2 ");
      Serial.println(i);
      delay(15);
    }
  }
}

// Функция для счетчика переключения режимов кнопкой
bool buttom_control(){
  // Читаем значение с кнопки
  int newValue = digitalRead(BUTTON);

  // Проверяем изменилось ли значения, сравнивая старое и новое
  if(newValue != oldValue)
  {
    // Если изменилось, то заменяем
    oldValue = newValue;
  }

  //Возвращаем значение
  delay(15);
  if(oldValue == HIGH) 
  {
    Serial.print("Положение кнопки - ");
    Serial.println(oldValue);
    return true;
  } else {
    Serial.print("Положение кнопки - ");
    Serial.println(oldValue);
    return false;}
}


// void move_servo(Servo *servo, uint8_t PtStart, uint8_t PtEnd)
// {
//    servo->write(PtStart);
//    delay(300);
//    servo->write(PtEnd);
//    delay(300);
//    servo->write(PtStart);
//    delay(300);
// }