// Даём понятное имя пину направления вращения левого мотора
#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
 
// Настраиваем направление вращения моторов
// Если мотор вращается в обратную сторону, меняем false на true
constexpr bool ML_INVERT = false;
constexpr bool MR_INVERT = false;
 
// Задаем порог, ниже которого считаем, что датчик видит чёрную линию
constexpr int LINE_BLACK = 40;

// Задаём скорост моторов
constexpr int M_SPEED = 150;

void setup() {
  // Настраиваем пины управления моторами в режим выхода
  pinMode(PIN_ML_DIR, OUTPUT);
  pinMode(PIN_ML_SPEED, OUTPUT);
  pinMode(PIN_MR_DIR, OUTPUT);
  pinMode(PIN_MR_SPEED, OUTPUT);
  // Настраиваем пины управления сервоприводами в режим выхода
  pinMode(PIN_LINE_L, INPUT);
  pinMode(PIN_LINE_R, INPUT);
  // Делаем небольшую задержку, чтобы успеть поставить робота
  delay(10000);

  // Запускаем алгоритм езды до перекрестка
  goToCrossroad(M_SPEED);
}
 
void loop() {
  // Функция loop() ничего не делает
}
 
// Функция езды до перекрестка
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 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));
}
 
// Считывание левого датчика линии
int readLineL() {
  return map(analogRead(PIN_LINE_L), 0, 1024, 100, 0);
}
 
// Считывание правого датчика линии
int readLineR() {
  return map(analogRead(PIN_LINE_R), 0, 1024, 100, 0);
}