Робот-следопыт с ИИ на Arduino Nano: PID + лабиринт (Left-Hand Rule)

Компоненты и расходники

Arduino Nano	×1
Bluetooth-модуль (Master/Slave, например HC-05/06)	×1
7 ИК-рефлективных датчиков (линейные + 2 боковых)	×1 комплект
2 сервопривода непрерывного вращения (прямой привод колёс)	×2
Аккумулятор 4,8 В (NiMH или Li-Ion с понижением)	×1

Программное обеспечение

• Arduino IDE

О проекте

После робота-манипулятора захотел сделать робота с элементами ИИ — решающего лабиринт по правилу левой руки. Это не просто следование за линией, а принятие решений на перекрёстках.

Зачем ИИ в роботе-следопыте?

• Сборка/производственные линии.
• Автономные пассажирские модули.
• Освоение навигации в неструктурированной среде.

Мой прототип — первый шаг к ИИ. Ресурсы ограничены, но код рабочий и понятный.

ИК-датчики: расположение и логика

• 5 центральных датчиков (LFS0–LFS4) — для PID-следования. Выдают 0/1. Только центральный над линией = 1. Возможные паттерны (11 шт.):
  10000, 11000, 01000, 01100, 00100, 00110, 00010, 00011, 00001, 00000, 11111
• 2 боковых датчика (Far Left, Far Right) — аналоговые, детектят повороты/перекрёстки заранее.

Массив LFSensor[5] хранит состояния центральных, переменные farLeftSensor, farRightSensor — боковых.

Правило левой руки (Left-Hand Rule) для лабиринта

Держи левую руку у стены — всегда поворачивай налево, где можно. Типы перекрёстков (8):

• Cross (+) — влево.
• T-образный — влево.
• Только вправо — вправо.
• Только влево — влево.
• Прямо или влево — влево.
• Прямо или вправо — прямо.
• Тупик — разворот (U-turn).
• Финиш — стоп.

Проблема: по 5 центральным датчикам «перекрёсток» и «T» выглядят одинаково (все 1). Решение: проехать «дополнительный дюйм» и перечитать датчики.

• Если после дюйма снова линия — это Cross → влево.
• Если линии нет — T → влево.
• Если снова все 1 — финиш → стоп.

Аналогично для поворотов: проехать дюйм, проверить, есть ли линия прямо.

Архитектура кода (модульная)

• RoBot_Maze_Solve_2 — main (setup, loop, две фазы: обучение и оптимизация).
• Functions — mazeSolve, recIntersection, mazeEnd, followingLine, simplifyPath, mazeOptimization, mazeTurn.
• GeneralFunctions — ledBlink, BT-команды, ручное управление, PID, тест датчиков.
• sensorFuntions — readLFSsensors (классификация режимов: FOLLOWING_LINE, CONT_LINE, LEFT_TURN, RIGHT_TURN, NO_LINE).
• RobotDefines.h — пины, константы, PID, массивы пути.
• MotorFuntions — управление сервами (PID, повороты, доп. дюйм).

Основной скетч (RoBot_Maze_Solve_2)

/* 7 sensors Smart Robot - Maze Solver & Line Follower
   PID tunable via BT. Based on Nano Mouse (Michael Backus)
   Line follow: samvrit.tk PID tutorial. Path solving: Patrick McCabe.
   Marcelo Jose Rovai - 23 Apr 2016 - mjrobot.org
*/
#include 
#include "robotDefines.h"
#include 

SoftwareSerial BT1(10, 11); // Rx, Tx
String command, device;

void setup() {
  Serial.begin(9600); BT1.begin(9600);
  pinMode(ledPin, OUTPUT);
  pinMode(buttonPin, INPUT_PULLUP);
  pinMode(lineFollowSensor0, INPUT); // 12
  pinMode(lineFollowSensor1, INPUT); // 18
  pinMode(lineFollowSensor2, INPUT); // 17
  pinMode(lineFollowSensor3, INPUT); // 16
  pinMode(lineFollowSensor4, INPUT); // 19
  pinMode(farRightSensorPin, INPUT); // A0
  pinMode(farLeftSensorPin, INPUT);  // A1

  leftServo.attach(5); rightServo.attach(3);
  BT1.print("check the PID constants to be sent to Robot\n");
  while (digitalRead(buttonPin) && !mode) { checkBTcmd(); manualCmd(); command = ""; }
  checkPIDvalues();
  mode = STOPPED; status = 0;
}

void loop() {
  ledBlink(1); BT1.println("Start First Pass"); Serial.println("Start First Pass");
  readLFSsensors(); mazeSolve(); // Фаза 1: обучение
  ledBlink(2); BT1.println("End First Pass"); Serial.println("End First Pass");
  while (digitalRead(buttonPin) && !mode) { checkBTcmd(); manualCmd(); command = ""; }
  BT1.println("Starting 2nd Pass"); Serial.println("Starting 2nd Pass");
  pathIndex = 0; status = 0; mazeOptimization(); // Фаза 2: быстрый проезд
  ledBlink(3); BT1.println("End 2nd Pass"); Serial.println("End 2nd Pass");
  while (digitalRead(buttonPin) && !mode) { checkBTcmd(); manualCmd(); command = ""; }
  mode = STOPPED; status = 0; pathIndex = 0; pathLength = 0;
}

Ключевые функции

mazeSolve() — фаза обучения

void mazeSolve() {
  while (!status) {
    readLFSsensors();
    switch (mode) {
      case NO_LINE: motorStop(); goAndTurn(LEFT, 180); recIntersection('B'); break;
      case CONT_LINE: runExtraInch(); readLFSsensors();
        if (mode != CONT_LINE) { goAndTurn(LEFT, 90); recIntersection('L'); }
        else mazeEnd(); break;
      case RIGHT_TURN: runExtraInch(); readLFSsensors();
        if (mode == NO_LINE) { goAndTurn(RIGHT, 90); recIntersection('R'); }
        else recIntersection('S'); break;
      case LEFT_TURN: goAndTurn(LEFT, 90); recIntersection('L'); break;
      case FOLLOWING_LINE: followingLine(); break;
    }
  }
}

simplifyPath() — упрощение пути (LBL→S и т.д.)

void simplifyPath() {
  if (pathLength < 3 || path[pathLength-2] != 'B') return;
  int totalAngle = 0;
  for (int i=1; i<=3; i++) {
    switch (path[pathLength-i]) {
      case 'R': totalAngle += 90; break;
      case 'L': totalAngle += 270; break;
      case 'B': totalAngle += 180; break;
    }
  }
  totalAngle %= 360;
  switch (totalAngle) {
    case 0: path[pathLength-3] = 'S'; break;
    case 90: path[pathLength-3] = 'R'; break;
    case 180: path[pathLength-3] = 'B'; break;
    case 270: path[pathLength-3] = 'L'; break;
  }
  pathLength -= 2;
}

mazeOptimization() — вторая фаза (по оптимальному пути)

void mazeOptimization() {
  while (!status) {
    readLFSsensors();
    switch (mode) {
      case FOLLOWING_LINE: followingLine(); break;
      case CONT_LINE: case LEFT_TURN: case RIGHT_TURN:
        if (pathIndex >= pathLength) mazeEnd();
        else { mazeTurn(path[pathIndex]); pathIndex++; }
        break;
    }
  }
}

PID-следование за линией

void calculatePID() {
  P = error; I += error; D = error - previousError;
  PIDvalue = (Kp*P) + (Ki*I) + (Kd*D);
  previousError = error;
}
void motorPIDcontrol() {
  int leftMotorSpeed = 1500 - (iniMotorPower+adj) - PIDvalue;
  int rightMotorSpeed = 1500 + iniMotorPower - PIDvalue;
  constrain(leftMotorSpeed, 1000, 2000);
  constrain(rightMotorSpeed, 1000, 2000);
  leftServo.writeMicroseconds(leftMotorSpeed);
  rightServo.writeMicroseconds(rightMotorSpeed);
}

Управление по Bluetooth (настройка PID, ручной тест)

Команды: g (go), s (stop), f/b/l/r (движение), p/i/d + цифра (Kp/Ki/Kd).

Схема подключения