Робот-трассоискатель с ПИД-регулятором на Arduino UNO: набор KittenBot

Комплектующие (набор KittenBot)

• Алюминиевый анодированный шасси KittenBot — 1 шт.
• Базовая плата RosBot (встроенные 2× H-моста) — 1 шт.
• Опорные шариковые колёса (пара) — 1 компл.
• Резиновые колёса с втулками под моторы (пара) — 1 компл.
• 5-канальный ИК-датчик слежения за линией — 1 шт.
• DC-моторы (в комплекте с шасси) — 2 шт.
• Крона 9 В — 1 шт.
• Arduino UNO — 1 шт. (устанавливается на RosBot)

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

• Arduino IDE

Особенности конструкции

Плата RosBot — это Arduino-совместимая база с двумя встроенными H-мостами. Никаких внешних драйверов двигателей не требуется: моторы подключаются напрямую к клеммам A+/A− и B+/B−. Плата выдаёт больший ток, чем чистый Arduino UNO.

Шасси из анодированного алюминия имеет множество отверстий (4.8 мм, совместимых с LEGO Technic) — удобно для доработок.

5-канальный ИК-датчик обеспечивает высокую точность определения позиции линии: центральный и два крайних сенсора с каждой стороны.

Сборка (≈15 минут)

1. Закрепите моторы по бокам шасси, надейте резиновые колёса.
2. Установите 5-канальный ИК-датчик на переднюю часть шасси.
3. Прикрепите плату RosBot на шасси, соедините провода.

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

• VCC → 5V
• GND → GND
• T1–T4 → A0–A3
• T5 → SDA (A4)

Моторы: левый → A+/A−, правый → B+/B−.

Программная логика

Используется конечный автомат (stateMachine), анализирующий 5-битную маску сенсоров. Каждому состоянию соответствует смещение (bias) от центра линии.

ПИД-регулятор вычисляет корректирующее воздействие:

float Kp = 25, Ki = 0.15, Kd = 1200;
float error, errorLast, erroInte;

float calcPid(float input) {
  error = error * 0.7 + input * 0.3; // фильтр
  float errorDiff = error - errorLast;
  erroInte = constrain(erroInte + error, -50, 50);
  float output = Kp * error + Ki * erroInte + Kd * errorDiff;
  errorLast = error;
  return output;
}

Скорости моторов: baseSpeed ± pidOutput. При потере линии (>10 циклов) робот останавливается.

Код (основной скетч)

#include <Adafruit_NeoPixel.h>
#define S_NULL 0
#define S_ONTRACE 1

Adafruit_NeoPixel pixels(4, 4, NEO_GRB + NEO_KHZ800);

void doDcSpeed(int spdL, int spdR) {
  spdR = -spdR;
  if (spdL < 0) { analogWrite(5, 0); analogWrite(6, -spdL); }
  else { analogWrite(5, spdL); analogWrite(6, 0); }
  if (spdR < 0) { analogWrite(9, 0); analogWrite(10, -spdR); }
  else { analogWrite(9, spdR); analogWrite(10, 0); }
}

int bias = 0, outlineCnt = 0;

void stateMachine(int a) {
  switch (a) {
    case B00000: case B11111: outlineCnt++; break;
    case B00010: case B00110: outlineCnt = 0; pixels.setPixelColor(2, pixels.Color(0,50,0)); bias = 1; break;
    case B00001: case B00011: outlineCnt = 0; pixels.setPixelColor(2, pixels.Color(0,200,0)); bias = 2; break;
    case B00100: outlineCnt = 0; pixels.setPixelColor(2, pixels.Color(0,0,20)); bias = 0; break;
    case B01000: case B01100: outlineCnt = 0; pixels.setPixelColor(2, pixels.Color(50,0,0)); bias = -1; break;
    case B10000: case B11000: outlineCnt = 0; pixels.setPixelColor(2, pixels.Color(200,0,0)); bias = -2; break;
    default: Serial.println(a, BIN); outlineCnt++; break;
  }
  pixels.setPixelColor(0, pixels.Color(outlineCnt*10, 0, 0));
  if (outlineCnt > 10) { doDcSpeed(0,0); }
  else { float ff = 150; float ctrl = calcPid(bias); doDcSpeed(ff-ctrl, ff+ctrl); }
  pixels.show();
}

int echoTrace() {
  int ret = 0, a[5];
  for (int i=0; i<5; i++) {
    a[i] = constrain((1025 - analogRead(A0+i))/10 - 4, 0, 20);
    if (a[i] > 2) ret |= (0x1 << i);
  }
  return ret;
}

void setup() {
  Serial.begin(115200);
  pixels.begin();
}

void loop() {
  delay(5);
  int pos = echoTrace();
  stateMachine(pos);
}

Ресурсы

Полный код и 3D-модели: github.com/KittenBot/LinefollowRobot