// Библиотека для работы с сервоприводами
#include <Servo.h>
 
// Даём понятное имя пину направления вращения левого мотора
#define PIN_ML_DIR 7
// Даём понятное имя пину управления скоростью вращения левого мотора
#define PIN_ML_SPEED 6
// Даём понятное имя пину направления вращения правого мотора
#define PIN_MR_DIR 4
// Даём понятное имя пину управления скоростью вращения правого мотора
#define PIN_MR_SPEED 5
// Даём понятное имя пину левого датчика линии
#define PIN_LINE_L A0
// Даём понятное имя пину правого датчика линии
#define PIN_LINE_R A1
// Даём понятные имена пинам левого энкодера A и B
#define PIN_ENC_LA 2
#define PIN_ENC_LB 12
// Даём понятные имена пинам правого энкодера A и B
#define PIN_ENC_RA 3
#define PIN_ENC_RB 13
// Даём понятное имя пину ИК-дальномера
#define PIN_IRR A2
// Даём понятные имена пинам УЗ-дальномера TRIG и ECHO
#define PIN_USR_T 11
#define PIN_USR_E 10
// Даём понятное имя пину сервопривода головы
#define PIN_SRV_HEAD 8
// Даём понятное имя пину сервомотора захвата
#define PIN_SRV_GRIP 9
// Даём понятное имя пину пищалки
#define PIN_BUZZER A3
 
// Создаём объект для работы сервоприводом головы
Servo head;
// Создаём объект для работы сервоприводом захвата
Servo grip;
 
// Задаём углы для верхнего и нижнего положения захвата
// Значения углов подбираются экспериментально в диапазоне от 0 до 180
constexpr int GRIP_UP = 175;
constexpr int GRIP_DOWN = 75;
 
// Настраиваем направление вращения моторов
// Если мотор вращается в обратную сторону, меняем false на true
constexpr bool ML_INVERT = false;
constexpr bool MR_INVERT = false;
 
// Задаем порог, ниже которого считаем, что датчик видит чёрную линию
constexpr int LINE_BLACK = 20;
 
// Задаём скорост вращения мотора при повороте
constexpr int TURN_SPEED = 128;
// Задаём время для сьезда с линии при повороте в миллисекундах
constexpr int TURN_WAIT  = 150;
// Задаем время поворота робота назад для выравнивания по линии
constexpr int TURN_BACK  = 200;
 
// Задаём скорост моторов для движения по линии
constexpr int DRIVE_SPEED = 150;
 
void setup() {
  // Настраиваем пины управления моторами в режим выхода
  pinMode(PIN_ML_DIR, OUTPUT);
  pinMode(PIN_ML_SPEED, OUTPUT);
  pinMode(PIN_MR_DIR, OUTPUT);
  pinMode(PIN_ML_SPEED, OUTPUT);
  // Настраиваем пины управления сервоприводами в режим выхода
  pinMode(PIN_SRV_HEAD, OUTPUT);
  pinMode(PIN_SRV_GRIP, OUTPUT);
  // Настраиваем пины УЗ-дальномера
  // пин TRIG в режим выхода, пин ECHO в режим входа
  pinMode(PIN_USR_T, OUTPUT);
  pinMode(PIN_USR_E, INPUT);
  // Настраиваем пины датчиков линии в режим входа
  pinMode(PIN_LINE_L, INPUT);
  pinMode(PIN_LINE_R, INPUT);
  // Настраиваем пин ИК-дальномера в режим входа
  pinMode(PIN_IRR, INPUT);
  // Настраиваем пины энкодеров в режим входа
  pinMode(PIN_ENC_LA, INPUT);
  pinMode(PIN_ENC_LB, INPUT);
  pinMode(PIN_ENC_RA, INPUT);
  pinMode(PIN_ENC_RB, INPUT);
  // Инициализируем сервопривод головы
  head.attach(PIN_SRV_HEAD);
  // Инициализируем сервопривода захвата
  head.attach(PIN_SRV_GRIP);
 
  // Запускаем выполнение алгоритма
  runRobot();
}
 
void loop() {
  // Функция loop() не используется
}
 
// Получение показаний с ИК-дальномера
int readDistIRR() {
  // Считываем показания с ИК-дальномера в отчётах АЦП
  int value = analogRead(PIN_IRR);
  // Конвертируем показания в напряжение
  float voltage = value * (5.0 / 1024.0);
  // Конвертируем напряжение в расстояние в см
  float distance = 29.988 * pow(voltage, -1.173);
  // Возвращаем полученное значение
  return int(distance);
}
 
// Функция считывания левого датчика линии
int readLineL() {
  return map(analogRead(PIN_LINE_L), 0, 1024, 100, 0);
}
 
// Функция считывания правого датчика линии
int readLineR() {
  return map(analogRead(PIN_LINE_R), 0, 1024, 100, 0);
}
 
// Функция поднятия захвата
void gripUp() {
  // Плавно поднимаем захват из нижнего положения в верхнее
  for (int i = GRIP_DOWN; i < GRIP_UP; i += 5) {
    grip.write(i);
    delay(50);
  }
}
 
// Функция опускания захвата
void gripDown() {
  // Плавно опускаем захват из верхнего положения в нижнее
  for (int i = GRIP_UP; i > GRIP_DOWN; i -= 5) {
      grip.write(i);
      delay(50);
    }
}
 
// Управление движением робота
// Указываем скорость левого и правого мотора от 0 до 255,
// где 0 — остановка, а 255 — максимальная скорость
// Знак задаёт направление вращения: плюс — вперёд, минус — назад
void motorsDrive(int ml, int mr) {
  // Определяем направление вращения левого мотора
  bool mlDir = ml < 0;
  // Определяем направление вращения правого мотора
  bool mrDir = mr < 0;
  // При необходимости инвертируем направление моторов
  if (ML_INVERT) mlDir = !mlDir;
  if (MR_INVERT) mrDir = !mrDir;
  // Задаём направление вращения моторов
  digitalWrite(PIN_ML_DIR, mlDir);
  digitalWrite(PIN_MR_DIR, mrDir);
  // Задаём скорость вращения моторов
  analogWrite(PIN_ML_SPEED, abs(ml));
  analogWrite(PIN_MR_SPEED, abs(mr));
}
 
// Функция поворота направо
void turnR(int speed, int waitTime) {
  // Включаем левый мотор
  motorsDrive(speed, 0);
  // Даем роботу время съехать с линии
  delay(waitTime);
  // Ждём, пока левый датчик снова увидит линию
  while (readLineL() > LINE_BLACK) {
    delay(10);
  }
  // Останавливаемся после обнаружения линии
  motorsDrive(0, 0);
  delay(500);
  // Немного доворачиваем назад, чтобы выровнять робота
  motorsDrive(0, speed);
  delay(TURN_BACK);
  // Останавливаем моторы
  motorsDrive(0, 0);
  delay(250);
}
 
// Функция поворота налево
void turnL(int speed, int waitTime) {
  // Включаем правый мотор
  motorsDrive(0, speed);
  // Даем роботу время съехать с линии
  delay(waitTime);
  // Ждём, пока правый датчик снова увидит линию
  while (readLineR() > LINE_BLACK) {
    delay(10);
  }
  // Останавливаемся после обнаружения линии
  motorsDrive(0, 0);
  delay(500);
  // Немного доворачиваем назад, чтобы выровнять робота
  motorsDrive(speed, 0);
  delay(TURN_BACK);
  // Останавливаем моторы
  motorsDrive(0, 0);
  delay(250);
}
 
// Функция возврата после поворота направо
void turnBackR(int speed, int waitTime) {
  // Включаем левый мотор в обратную сторону
  motorsDrive(-speed, 0);
  // Ждём, пока левый датчик снова увидит линию
  while (readLineL() > LINE_BLACK) {
    delay(10);
  }
  // Даем время, чтобы выровнять робота
  delay(waitTime);
  // Останавливаем моторы
  motorsDrive(0, 0);
  delay(250);
}
 
// Функция возврата после поворота налево
void turnBackL(int speed, int waitTime) {
  // Включаем левый мотор в обратную сторону
  motorsDrive(0, -speed);
  // Ждём, пока левый датчик снова увидит линию
  while (readLineR() > LINE_BLACK) {
    delay(10);
  }
  // Даем время, чтобы выровнять робота
  delay(waitTime);
  // Останавливаем моторы
  motorsDrive(0, 0);
  delay(250);
}
 
// Функция разворота на 180° по часовой стрелке (направо)
void turn180R(int speed, int waitTime) {
  // Включаем моторы в разные стороны
  motorsDrive(speed, -speed);
  // Даем роботу время съехать с линии
  delay(waitTime);
  // Ждём, пока датчик снова увидит линию
   while (readLineL() > LINE_BLACK) {
    delay(10);
  }
  // Останавливаемся после обнаружения линии
  motorsDrive(0, 0);
  delay(200);
  // Немного доворачиваем назад, чтобы выровнять робота
  motorsDrive(-speed, speed);
  delay(TURN_BACK);
  // Останавливаем моторы
  motorsDrive(0, 0);;
  delay(250);
}
 
// Функция разворота на 180° против часовой стрелке (налево)
void turn180L(int speed, int waitTime) {
  // Включаем моторы в разные стороны
  motorsDrive(-speed, speed);
  // Даем роботу время съехать с линии
  delay(waitTime);
  // Ждём, пока датчик снова увидит линию
   while (readLineR() > LINE_BLACK) {
    delay(10);
  }
  // Останавливаемся после обнаружения линии
  motorsDrive(0, 0);
  delay(200);
  // Немного доворачиваем назад, чтобы выровнять робота
  motorsDrive(speed, -speed);
  delay(TURN_BACK);
  // Останавливаем моторы
  motorsDrive(0, 0);;
  delay(250);
}
 
// Функция езды до перекрестка
void goToCrossroad(int speed) {
  // Задаём пропорциональный коэффициент П-регулятора
  int Kp = 10;
  // Считываем показания датчиков линии
  int leftLine = readLineL();
  int rightLine = readLineR();
  // Следуем по линии, пока не встретим перекрёсток
  while (leftLine > LINE_BLACK && rightLine > LINE_BLACK) {
    // Считываем показания датчиков линии
    leftLine = readLineL();
    rightLine = readLineR();
    // Рассчитываем ошибку
    int err = leftLine - rightLine;
    // Формируем управляющий сигнал
    int out = Kp * err;
    // Ограничиваем максимальное и минимальное значения управляющего сингала
    out = constrain(out, -100, 100);
    // Регулируем скорость моторов в соответствии с управляющим сигналом
    motorsDrive(speed + out, speed - out);
  }
  // Даем роботу немного времени заехать за линию
  motorsDrive(speed, speed);
  delay(500);
  // Останавливаем моторы
  motorsDrive(0, 0);
}
 
// Функция для звукового сигнала
void beep(int n) {
  // Включаем и выключам пищалку в цикле n раз
  for (int i = 0; i < n; i++) {
    // Включаем пищалку на 250 миллисекунд
    digitalWrite(PIN_BUZZER, HIGH);
    delay(250);
    // Выключаем пищалку на 250 миллисекунд
    digitalWrite(PIN_BUZZER, LOW);
    delay(250);
  }
}

// Функция с алгоритмом выполнения задания
void runRobot() {
  // Количество перекрёстков
  const int n = 5;
  // Расположение кубиков
  bool cubes[] = {0, 0, 0, 0, 0};
  // Сигнализируем о начале выполнения алгоритма
  beep(2);
  // Отслеживаем текущий перекрёсток
  int i = 0;
}