// Даём понятное имя пину направления вращения левого мотора
#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 FOLLOW_SPEED = 150;

// Задаём время, в течение которого робот делжен ехать
constexpr int FOLLOW_TIME = 10000;

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);
  // Делаем паузу 10 секунд, чтобы успеть поставить робота на поле
  delay(10000);
  
  // Даем команду роботу ехать по линии
   followLine(FOLLOW_SPEED, FOLLOW_TIME);
}
 
void loop() {
  // В этом примере loop() ничего не делает
}
 
// Функция следования по линии
void followLine(int speed, int followTime) {
  // Задаём пропорционаальный коэффициеент П-регулятора 
  int Kp = 5;
  // Сохраняем время начала движения
  int startTime = millis();
  // Следуем по линии в течение задангого времени waitTime
  while (millis() - startTime < followTime) {
    // Считываем показания датчиков линии
    int leftLine = readLineL();
    int rightLine = readLineR();
    // Расчитываем оибку
    int err = leftLine - rightLine;
    // Формируем управляющий сигнал
    int out = Kp * err ; 
    // Ограничиваем максимальное и минимальное значения управляющего сингала
    out = constrain(out, 0, 200);
    // Ругулируем скорость моторов в соответсвии с управляющим сигналом 
    motorsDrive(speed + out, speed - out);
  }
  // Останавливаем моторы
  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);
}