// Подключаем библиотеку для работы с сервоприводом
#include <Servo.h>
// Подключаем библиотеку для работы с кнопкой
#include <TroykaButton.h>

// Создаём объект сервопривода с клешнёй захвата
Servo servoGripper;

// Создаём объект кнопки на пине 2
TroykaButton button(2);

// Даём понятное имя светодиоду на пине 10
constexpr uint8_t LED_PIN = 10;

// Даём понятное имя потенциометру на пине A0
constexpr uint8_t POT_PIN = A0;

// Даём понятное имя пину A1 с сервоприводом клешни
constexpr uint8_t SERVO_GRIPPER_PIN = A1;

// Задаём максимально доступные углы поворота сервопривода клешни
constexpr int ANGLE_GRIPPER_OPEN = 30;
constexpr int ANGLE_GRIPPER_CLOSED = 90;

// Вычисляем средний угол поворота сервопривода клешни
constexpr int ANGLE_GRIPPER_MID = (ANGLE_GRIPPER_OPEN + ANGLE_GRIPPER_CLOSED) / 2;

// Создаём константу для хранения паузы сервопривода при каждом градусе поворота
constexpr int DELAY_SWEEP = 20;
// Создаём константу для хранения паузы сервопривода между поворотами
constexpr int DELAY_STOP = 1000;

// Создаём перечисление состояний захвата с соответствующей переменной
enum {
  GRIPPER_ON, // Захват включен
  GRIPPER_OFF, // Захват выключен
} gripperState;

// Прототип функции плавной установки угла сервопривода
void servoWriteSmooth(int angleNew, int delaySweep = 20);

void setup() {
  // Инициализируем кнопку
  button.begin();
  // Настраиваем пин со светодиодом в режим выхода
  pinMode(LED_PIN, OUTPUT);
  // Настраиваем пин с потенциометром в режим входа
  pinMode(POT_PIN, INPUT);
  // Включаем световую индикацию старта работы
  ledStart();
  // Подключаем сервопривод с клешнёй захвата
  servoGripper.attach(SERVO_GRIPPER_PIN);
  // Устанавливаем начальное состояние клешни в среднее положение
  servoGripper.write(ANGLE_GRIPPER_MID);
  // Выжидаем паузу после инициализации клешни
  delay(1000);
  // Устанавливаем режим «Захват выключен»
  gripperState = GRIPPER_OFF;
}

void loop() {
  // Если захват включен
  if (gripperState == GRIPPER_ON) {
    // Переходим в функцию обработки режима «Захват включен»
    handleGripperOn();
  }
  // Если захват выключен
  if (gripperState == GRIPPER_OFF) {
    // Переходим в функцию обработки режима «Захват выключен»
    handleGripperOff();
  }
}

// Функция обработки режима «Захват выключен»
void handleGripperOff() {
  // Гасим светодиод
  digitalWrite(LED_PIN, LOW);
  // Ожидаем нажатие на кнопку
  do {
    // Считываем состояние с кнопки
    button.read();
  } while (!button.isClick());
  // Устанавливаем режим «Захват включен»
  gripperState = GRIPPER_ON;
}

// Функция обработки режима «Захват включен»
void handleGripperOn() {
  // Зажигаем светодиод
  digitalWrite(LED_PIN, HIGH);
  // Плавно устанавливаем состояние клешни из положения потенциометра
  remoteHandlerGripperPotSmooth();
  // Ожидаем нажатие на кнопку
  do {
    // Устанавливаем состояние клешни из положения потенциометра
    remoteHandlerGripperPot();
    // Считываем состояние с кнопки
    button.read();
  } while (!button.isClick());
  // Устанавливаем плавно состояние клешни в закрытое положение
  servoWriteSmooth(ANGLE_GRIPPER_CLOSED);
  // Устанавливаем режим «Захват выключен»
  gripperState = GRIPPER_OFF;
}

// Функция установки состояние клешни из положения потенциометра
void remoteHandlerGripperPot() {
  // Считываем аналоговый сигнал с потенциометра
  int rotation = analogRead(POT_PIN);
  // Преобразуем диапазон значений с потенциометра [0; 1023]
  // в диапазон значений угла сервопривода клешни [ANGLE_GRIPPER_OPEN; ANGLE_GRIPPER_CLOSED]
  int angle = map(rotation, 0, 1023, ANGLE_GRIPPER_OPEN, ANGLE_GRIPPER_CLOSED);
  // Выдаём результат на клешню
  servoGripper.write(angle);
}

// Функция плавной установки состояние клешни из положения потенциометра
void remoteHandlerGripperPotSmooth() {
  // Считываем аналоговый сигнал с потенциометра
  int rotation = analogRead(POT_PIN);
  // Преобразуем диапазон значений с потенциометра [0; 1023]
  // в диапазон значений угла сервопривода клешни [ANGLE_GRIPPER_OPEN; ANGLE_GRIPPER_CLOSED]
  int angle = map(rotation, 0, 1023, ANGLE_GRIPPER_OPEN, ANGLE_GRIPPER_CLOSED);
  // Выдаём результат на клешню
  servoWriteSmooth(angle);
}

// Функция светодиодной индикации старта работы
void ledStart() {
  // Перебираем значения от 0 до 2
  for (int i = 0; i < 3; i++) {
    // Зажигаем светодиод
    digitalWrite(LED_PIN, HIGH);
    // Ждём 300 мс
    delay(300);
    // Гасим светодиод
    digitalWrite(LED_PIN, LOW);
    // Ждём 300 мс
    delay(300);
  }
}

// Функция плавной установки угла сервопривода
void servoWriteSmooth(int angleNew, int delaySweep) {
  // Считываем текущий угол сервопривода
  int angleNow = servoGripper.read();
  // Если текущий угол меньше нового
  if (angleNow < angleNew) {
    // Увеличиваем значения угла от текущего до нового
    for (int angle = angleNow; angle <= angleNew; angle++) {
      // Отправляем текущий угол на сервопривод
      servoGripper.write(angle);
      // Выжидаем паузу на каждом градусе поворота
      delay(delaySweep);
    }
  } else if (angleNow > angleNew) {
    // Если текущий угол меньше нового
    // Уменьшаем значения угла от текущего до нового
    for (int angle = angleNow; angle >= angleNew; angle--) {
      // Отправляем текущий угол на сервопривод
      servoGripper.write(angle);
      // Выжидаем паузу на каждом градусе поворота
      delay(delaySweep);
    }
  } else if (angleNow == angleNew) {
    // Если текущий угол совпадает с новым
    // Ничего не делаем и выходим из функции
    return true;
  }
}