Прошивки для роботов с объяснениями

Набор базовых «кирпичиков», из которых собирается программа почти любого робота. Код на Arduino C++: работает на Arduino, ESP32 и RP2040 с минимальными правками номеров выводов.

1. Как устроена прошивка

У Arduino-программы две функции: setup() выполняется один раз при включении, loop() — бесконечно, тысячи раз в секунду. Хорошая прошивка робота — это loop(), который никогда не останавливается: на каждом проходе быстро опрашивает датчики, обновляет логику и выставляет моторы.

void loop() {
  readSensors();     // прочитать всё, что изменилось
  updateLogic();     // решить, что делать
  driveOutputs();    // применить к моторам, светодиодам
}

Если где-то внутри стоит delay(1000), робот на секунду «слепнет»: не видит препятствие, не реагирует на кнопку. Поэтому первое правило — отказ от delay().

2. millis() вместо delay()

millis() возвращает количество миллисекунд с момента включения. Вместо «подожди 500 мс» мы спрашиваем «прошло ли 500 мс с прошлого раза?».

const uint8_t LED_PIN = 13;
unsigned long lastBlink = 0;
bool ledOn = false;

void setup() {
  pinMode(LED_PIN, OUTPUT);
}

void loop() {
  unsigned long now = millis();

  // Задача 1: мигать раз в 500 мс
  if (now - lastBlink >= 500) {
    lastBlink = now;
    ledOn = !ledOn;
    digitalWrite(LED_PIN, ledOn);
  }

  // Задача 2, 3... выполняются в том же проходе, ничего не ждут
}

3. Кнопка с антидребезгом

Механический контакт при нажатии «дребезжит»: за 5–20 мс замыкается и размыкается десятки раз. Без фильтра одно нажатие засчитается как несколько. Решение: принимать новое состояние, только если оно стабильно держится заданное время.

const uint8_t BTN_PIN = 2;
const unsigned long DEBOUNCE_MS = 25;

bool stableState = HIGH;      // с INPUT_PULLUP отпущенная кнопка = HIGH
bool lastReading = HIGH;
unsigned long lastChange = 0;

void setup() {
  pinMode(BTN_PIN, INPUT_PULLUP);  // встроенная подтяжка, кнопка замыкает на GND
  Serial.begin(115200);
}

void loop() {
  bool reading = digitalRead(BTN_PIN);
  if (reading != lastReading) {          // сигнал дёрнулся — перезапускаем таймер
    lastChange = millis();
    lastReading = reading;
  }
  if (millis() - lastChange > DEBOUNCE_MS && reading != stableState) {
    stableState = reading;
    if (stableState == LOW) {            // зафиксировано нажатие
      Serial.println("Нажата");
    }
  }
}

INPUT_PULLUP включает внутренний резистор к питанию, поэтому внешний резистор не нужен, а логика инвертирована: нажата — LOW.

4. Мотор через драйвер TB6612

У TB6612 на каждый мотор три сигнала: IN1, IN2 задают направление, PWM — скорость. Вывод STBY должен быть в HIGH, иначе драйвер спит.

struct Motor {
  uint8_t in1, in2, pwm;

  void begin() {
    pinMode(in1, OUTPUT); pinMode(in2, OUTPUT); pinMode(pwm, OUTPUT);
  }

  // speed от -255 до 255: знак — направление, модуль — скорость
  void set(int speed) {
    speed = constrain(speed, -255, 255);
    digitalWrite(in1, speed > 0);
    digitalWrite(in2, speed < 0);
    analogWrite(pwm, abs(speed));
  }

  void brake() {
    digitalWrite(in1, HIGH); digitalWrite(in2, HIGH); analogWrite(pwm, 255);
  }
};

const uint8_t STBY = 9;
Motor left  {4, 5, 6};    // выводы 6 и 3 на Uno поддерживают ШИМ
Motor right {7, 8, 3};

void setup() {
  pinMode(STBY, OUTPUT);
  digitalWrite(STBY, HIGH);
  left.begin(); right.begin();
}

void drive(int forward, int turn) {   // удобное управление «газ + руль»
  left.set(forward + turn);
  right.set(forward - turn);
}

void loop() {
  drive(150, 0);   // прямо
}

Функция drive() реализует дифференциальное управление: робот поворачивает разницей скоростей колёс, как танк. При forward = 0 и ненулевом turn он крутится на месте.

5. Энкодер на прерываниях

Квадратурный энкодер выдаёт два сигнала, A и B, сдвинутые на четверть периода. Когда A переходит из LOW в HIGH, по уровню B видно направление вращения. Импульсов может быть тысячи в секунду, поэтому их ловят прерыванием: процессор бросает текущую работу и сразу выполняет короткую функцию-обработчик.

const uint8_t ENC_A = 2;   // на Uno прерывания есть только на выводах 2 и 3
const uint8_t ENC_B = 4;

volatile long ticks = 0;   // volatile: переменная меняется внутри прерывания

void onEncoderA() {
  // Обработчик должен быть максимально коротким: никаких Serial и delay
  if (digitalRead(ENC_B)) ticks--; else ticks++;
}

void setup() {
  pinMode(ENC_A, INPUT_PULLUP);
  pinMode(ENC_B, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENC_A), onEncoderA, RISING);
  Serial.begin(115200);
}

long readTicks() {
  noInterrupts();          // long — 4 байта, 8-битный МК читает его не за один такт:
  long t = ticks;          // без защиты прерывание может изменить число посередине чтения
  interrupts();
  return t;
}

void loop() {
  static unsigned long last = 0;
  static long lastTicks = 0;
  if (millis() - last >= 100) {
    long t = readTicks();
    float ticksPerSec = (t - lastTicks) * 10.0;   // за 100 мс → в секунду
    lastTicks = t;
    last = millis();
    Serial.println(ticksPerSec);
  }
}

Перевод в скорость колеса: об/с = тики_в_секунду / (импульсы_на_оборот_вала_мотора × передаточное_число). Например, энкодер 11 импульсов, редуктор 1:30 — один оборот колеса это 330 импульсов по одному фронту канала A.

6. Сервоприводы и плавное движение

Хобби-серво управляется импульсом каждые 20 мс: 1 мс — один крайний угол, 2 мс — другой, 1,5 мс — середина. Библиотека Servo делает это сама. Если просто выдать write(180), серва рванёт на полной скорости и тряхнёт всю конструкцию. Плавность делается так: цель задаём сразу, а текущий угол каждые несколько миллисекунд сдвигаем на градус к цели.

#include <Servo.h>     // для ESP32 — библиотека ESP32Servo

Servo arm;
float current = 90, target = 90;
const float SPEED_DEG_PER_SEC = 60;   // максимальная скорость сустава

void setup() {
  arm.attach(10);
  arm.write(current);
}

void moveTo(float deg) { target = constrain(deg, 0, 180); }

void loop() {
  static unsigned long last = millis();
  unsigned long now = millis();
  float dt = (now - last) / 1000.0;
  last = now;

  float step = SPEED_DEG_PER_SEC * dt;
  if (abs(target - current) <= step) current = target;
  else current += (target > current ? step : -step);
  arm.write((int)round(current));

  // пример: качаемся между 30 и 150 градусами
  if (current == target) moveTo(target == 30 ? 150 : 30);
}

Если несколько суставов должны прийти в точку одновременно, скорость каждого делают пропорциональной его пути — тогда движение руки выглядит естественным.

7. Ультразвуковой дальномер HC-SR04

const uint8_t TRIG = 11, ECHO = 12;

float readDistanceCm() {
  digitalWrite(TRIG, LOW);  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH); delayMicroseconds(10);   // запуск: импульс 10 мкс
  digitalWrite(TRIG, LOW);
  // ждём эхо максимум 25 мс (~4 м), иначе считаем, что ничего нет
  unsigned long us = pulseIn(ECHO, HIGH, 25000UL);
  if (us == 0) return -1;
  return us * 0.0343 / 2;   // скорость звука 343 м/с, путь туда и обратно
}

void setup() {
  pinMode(TRIG, OUTPUT); pinMode(ECHO, INPUT);
  Serial.begin(115200);
}

void loop() {
  static unsigned long last = 0;
  if (millis() - last >= 60) {   // не чаще ~16 раз/с: иначе ловим старое эхо
    last = millis();
    Serial.println(readDistanceCm());
  }
}

Показания шумят и иногда «выстреливают». Простой фильтр — медиана из трёх последних измерений: она выбрасывает одиночные выбросы, в отличие от среднего.

float median3(float a, float b, float c) {
  return max(min(a, b), min(max(a, b), c));
}

8. Контроль заряда аккумулятора

Напряжение 2S-аккумулятора (до 8,4 В) через делитель 30 кОм / 10 кОм подаём на аналоговый вход. АЦП Arduino Uno 10-битный: 0–1023 соответствует 0–5 В.

const uint8_t VBAT_PIN = A0;
const float DIVIDER = (30.0 + 10.0) / 10.0;   // коэффициент делителя = 4
const float VREF = 5.0;

float readBattery() {
  long sum = 0;
  for (int i = 0; i < 16; i++) sum += analogRead(VBAT_PIN);   // усреднение шума
  return (sum / 16.0) / 1023.0 * VREF * DIVIDER;
}

void loop() {
  float v = readBattery();
  if (v < 6.6) {        // 3,3 В на ячейку: пора домой
    // остановить моторы, мигать светодиодом, перестать ездить
  }
}

Под нагрузкой напряжение проседает, поэтому решение «аккумулятор сел» принимают, только если низкое значение держится несколько секунд.

9. Конечный автомат — «мозг» робота

Когда поведение усложняется, цепочки if превращаются в кашу. Конечный автомат (FSM) раскладывает поведение на состояния и переходы. В каждый момент робот находится ровно в одном состоянии и знает, что в нём делать и при каких условиях переходить в другое.

Пример: робот, объезжающий препятствия.

enum State { DRIVE, BACKUP, TURN, STOPPED };
State state = DRIVE;
unsigned long stateStart = 0;

void enter(State s) {          // единая точка смены состояния
  state = s;
  stateStart = millis();
}
unsigned long inState() { return millis() - stateStart; }

void updateLogic() {
  float dist = readDistanceCm();
  float vbat = readBattery();

  if (vbat < 6.6 && state != STOPPED) enter(STOPPED);  // приоритетное условие

  switch (state) {
    case DRIVE:
      drive(160, 0);
      if (dist > 0 && dist < 20) enter(BACKUP);
      break;
    case BACKUP:
      drive(-120, 0);
      if (inState() > 400) enter(TURN);
      break;
    case TURN:
      drive(0, 140);                       // разворот на месте
      if (inState() > 350) enter(DRIVE);   // лучше — по гироскопу до нужного угла
      break;
    case STOPPED:
      drive(0, 0);
      break;
  }
}

Преимущества: поведение легко нарисовать на бумаге кружками и стрелками, легко добавить новое состояние, а таймеры каждого состояния не мешают друг другу.

10. Команды по Serial или Bluetooth

Модуль HC-05 или встроенный Bluetooth ESP32 выглядит для программы как обычный Serial-порт. Простой разборщик команд вида M 150 -40 (газ и поворот):

char buf[32];
uint8_t len = 0;

void handleCommand(char *cmd) {
  int a, b;
  if (sscanf(cmd, "M %d %d", &a, &b) == 2) drive(a, b);
  else if (strcmp(cmd, "STOP") == 0) drive(0, 0);
  else Serial.println("? unknown");
}

void pollSerial() {
  while (Serial.available()) {
    char c = Serial.read();
    if (c == '\n' || c == '\r') {
      if (len > 0) { buf[len] = 0; handleCommand(buf); len = 0; }
    } else if (len < sizeof(buf) - 1) {
      buf[len++] = c;
    }
  }
}

11. Отладка и хорошие привычки

• Serial Plotter в Arduino IDE рисует графики, если выводить числа через пробел или запятую в одной строке. Идеально для настройки ПИД и фильтров.
• Одна функция — одна задача. Номера выводов — в константах в начале файла.
• Безопасное состояние при старте: моторы выключены, пока всё не проинициализировано.
• Аппаратный watchdog (сторожевой таймер микроконтроллера) перезагружает плату, если программа зависла.
• Логика и железо отдельно: функцию принятия решений можно проверить на компьютере, подставляя «фальшивые» показания датчиков.
• Git даже для маленьких прошивок: когда «вчера работало», можно посмотреть, что изменилось.
• Когда проект перерастает Arduino IDE — PlatformIO (VS Code), а для сложных систем — FreeRTOS (уже встроен в ESP32) или ROS 2 на одноплатнике.