Проект 14: Мастер-режим — полнофункциональный робот
Введение
Финальный проект! Объединяем ВСЕ наработки в одну мощную программу — робот Фобо с 4 режимами работы, переключаемыми кнопками со смартфона!
Режимы робота:
📏 Следование по линии (Line Following) — автономное движение по линии (Проект 9)
🚧 Объезд препятствий (Obstacle Avoidance) — автономная навигация с ультразвуком (Проект 7)
✋ Следование за рукой (Hand Following) — поддержание дистанции до объекта (Проект 6)
🎮 Ручное управление (Manual) — управление со стрелок F/B/L/R
Переключение режимов: Кнопки 1, 2, 3 (автономные режимы), 4 (стоп → ручной режим)
Дополнительно:
📡 Запрос датчиков — кнопка 9 (ультразвук + линия)
🛑 Экстренный стоп — кнопка 4 (возврат в ручной режим)
Что уже должно работать:
✅ Робот полностью собран (Проект 1)
✅ Моторы калиброваны (Проекты 2-3)
✅ Все датчики проверены (Проекты 5, 8, 10)
✅ Автономные режимы протестированы (Проекты 6, 7, 9)
✅ Bluetooth подключение работает (Проекты 12-13)
Если что-то не работает — вернитесь к соответствующему проекту!
Компоненты
Робот Фобо в сборе (Проект 1)
HM-10 Bluetooth подключён к D10 (RX), D11 (TX)
RCWL-9610A + SG92R (ультразвук на серво, D3/D7/D9)
3 датчика линии (A0, A1, A2)
L298N + 4 мотора (D2, D4, D5, D6, D8, D12)
2x 18650 батареи заряженные
Смартфон/компьютер с Chrome/Edge/Opera
Примечание
Все компоненты уже установлены и протестированы в предыдущих проектах. Никаких дополнительных подключений не требуется!
Теория
Система переключения режимов
Робот работает как конечный автомат (state machine) — в каждый момент времени активен только один режим:
START → MANUAL MODE (ручное управление F/B/L/R)
│
┌────────────┼────────────┬────────────┐
│ │ │ │
Кнопка 1 Кнопка 2 Кнопка 3 Кнопка 4
│ │ │ │
▼ ▼ ▼ ▼
┌─────────┐ ┌─────────┐ ┌─────────┐ ┌─────────┐
│ LINE │ │OBSTACLE │ │ HAND │ │ E-STOP │
│FOLLOWING│ │AVOIDANCE│ │FOLLOWING│ │→ MANUAL │
└─────────┘ └─────────┘ └─────────┘ └─────────┘
│ │ │ │
└────────────┴────────────┴────────────┘
│
Кнопка 4 → возврат в MANUAL
Как это работает:
Робот стартует в Manual — ждёт команд со стрелок (F/B/L/R)
Нажатие кнопки 1 → переключение в Line Following → робот едет по линии
Нажатие кнопки 2 → переключение в Obstacle Avoidance → робот объезжает препятствия
Нажатие кнопки 3 → переключение в Hand Following → робот следует за рукой
Нажатие кнопки 4 → экстренный стоп → возврат в Manual
В автономных режимах (1-3) команды движения игнорируются — робот управляется датчиками!
Протокол команд
Переключение режимов (работает из любого режима):
Кнопка |
Команда |
Действие |
|---|---|---|
1 |
|
Line Following (следование по линии) |
2 |
|
Obstacle Avoidance (объезд препятствий) |
3 |
|
Hand Following (следование за рукой) |
4 |
|
Emergency Stop → Manual (экстренный стоп, возврат в ручной режим) |
9 |
|
Запрос данных датчиков (S1-S4) |
Ручное управление (работает только в режиме Manual):
Основные направления:
Кнопка |
Команды |
Действие |
|---|---|---|
▲ (стрелка вверх) |
|
Вперёд (нажата/отпущена) |
▼ (стрелка вниз) |
|
Назад (нажата/отпущена) |
◄ (стрелка влево) |
|
Поворот влево на месте (нажата/отпущена) |
► (стрелка вправо) |
|
Поворот вправо на месте (нажата/отпущена) |
Примечание
Uppercase (заглавная буква) = кнопка нажата → моторы работают
lowercase (строчная буква) = кнопка отпущена → моторы останавливаются
Это позволяет роботу двигаться пока кнопка удерживается, и останавливаться при отпускании.
Совет
В BLE Controller: кнопки 1-4, 9 находятся в центральной области (9 настраиваемых кнопок). Стрелки F/B/L/R настройте через Settings → Control Panel Settings.
Телеметрия датчиков
Запрос данных (кнопка 9):
Нажатие кнопки 9 запрашивает актуальные данные всех датчиков. Робот отправляет через Bluetooth 4 строки в формате:
S1:<значение>
S2:<значение>
S3:<значение>
S4:<значение>
Расшифровка:
Датчик |
Описание |
Диапазон значений |
|---|---|---|
S1 |
Ультразвуковой датчик (расстояние в см) |
0-400 (0 = вне диапазона) |
S2 |
Датчик линии слева (аналоговое значение) |
0-1023 |
S3 |
Датчик линии центр (аналоговое значение) |
0-1023 |
S4 |
Датчик линии справа (аналоговое значение) |
0-1023 |
Отображение данных:
При нажатии кнопки 9 в режиме RC Car данные автоматически отображаются в 4 полях над кнопками:
Ultrasonic: расстояние в см (0-400)
Line Left: значение левого датчика (0-1023)
Line Center: значение центрального датчика (0-1023)
Line Right: значение правого датчика (0-1023)
Примечание
В Serial Monitor (Arduino IDE) одновременно появится отладочная строка:
[BT] Sensor request
Sensors: D=25 L=28 C=30 R=26
Как использовать:
Откройте BLE Controller в браузере, режим RC Car
Подключитесь к роботу (BT05)
Нажмите кнопку 9 (в центральной панели)
Значения датчиков автоматически появятся в 4 полях над кнопками
Для повторного обновления — нажмите 9 ещё раз
Применение:
📊 Калибровка датчиков линии — проверьте значения S2, S3, S4 на белом/чёрном фоне
📏 Проверка ультразвука — убедитесь, что S1 показывает правильное расстояние
🐛 Отладка автономных режимов — смотрите что «видят» датчики в реальном времени
Код программы
Важно
Полный код (600+ строк) доступен в официальном репозитории:
Этот код включает: - ✅ Поддержку Bluetooth (HM-10) + ИК-пульт (VS1838B) - ✅ Ручное управление стрелками (F/B/L/R) - ✅ Конечные автоматы для автономных режимов (плавная работа серво) - ✅ Запрос датчиков (команда „9“ → S1-S4 данные) - ✅ IR timeout (авто-остановка через 200мс без сигнала)
Ключевые особенности кода
1. Структура программы:
/*
* Проект 14: Мастер-режим робота Фобо
* Управление: BLE Controller (Bluetooth) ИЛИ ИК-пульт
*
* Режимы:
* Кнопка 1 - Line Following (следование по линии)
* Кнопка 2 - Obstacle Avoidance (объезд препятствий)
* Кнопка 3 - Hand Following (следование за рукой)
* Кнопка 4 - Emergency Stop → Manual mode
* Кнопка 9 - Запрос данных датчиков (S1-S4)
*/
#include <SoftwareSerial.h>
#include <AlashMotorControlLite.h>
#include <AlashUltrasonic.h>
#include <Servo.h>
// ==================== BLUETOOTH ====================
const uint8_t BT_RX = 10;
const uint8_t BT_TX = 11;
SoftwareSerial bluetooth(BT_RX, BT_TX);
// ==================== MOTORS ====================
const uint8_t MOTOR_L_IN3 = 8;
const uint8_t MOTOR_L_IN4 = 12;
const uint8_t MOTOR_L_ENB = 6;
const uint8_t MOTOR_R_IN1 = 4;
const uint8_t MOTOR_R_IN2 = 2;
const uint8_t MOTOR_R_ENA = 5;
AlashMotorControlLite motorLeft(DIR_DIR_PWM, MOTOR_L_IN3, MOTOR_L_IN4, MOTOR_L_ENB);
AlashMotorControlLite motorRight(DIR_DIR_PWM, MOTOR_R_IN1, MOTOR_R_IN2, MOTOR_R_ENA);
// Скорости (минимумы: 45 вперёд, 65 поворот)
const int SPEED_FORWARD = 70;
const int SPEED_BACKWARD = 65;
const int SPEED_TURN = 80;
const int SPEED_SEARCH = 50;
// ==================== ULTRASONIC + SERVO ====================
const uint8_t TRIG_PIN = 3;
const uint8_t ECHO_PIN = 7;
const uint8_t SERVO_PIN = 9;
AlashUltrasonic ultrasonic(TRIG_PIN, ECHO_PIN);
Servo scanServo;
// Константы для серво и Hand Following
const int SERVO_CENTER = 90;
const int SERVO_ATTACH_DELAY = 15;
const int INIT_SERVO_WAIT = 300;
const int DISTANCE_TOO_CLOSE = 10;
const int DISTANCE_TOO_FAR = 30;
// ==================== LINE SENSORS ====================
const uint8_t LINE_LEFT = A0;
const uint8_t LINE_CENTER = A1;
const uint8_t LINE_RIGHT = A2;
// Калибровка (ЗАМЕНИТЕ НА СВОИ ЗНАЧЕНИЯ!)
const int LEFT_WHITE = 28, LEFT_BLACK = 36;
const int CENTER_WHITE = 30, CENTER_BLACK = 38;
const int RIGHT_WHITE = 25, RIGHT_BLACK = 33;
const int LEFT_THRESHOLD = (LEFT_WHITE + LEFT_BLACK) / 2;
const int CENTER_THRESHOLD = (CENTER_WHITE + CENTER_BLACK) / 2;
const int RIGHT_THRESHOLD = (RIGHT_WHITE + RIGHT_BLACK) / 2;
const bool LEFT_INVERTED = (LEFT_BLACK > LEFT_WHITE);
const bool CENTER_INVERTED = (CENTER_BLACK > CENTER_WHITE);
const bool RIGHT_INVERTED = (RIGHT_BLACK > RIGHT_WHITE);
// ==================== РЕЖИМЫ ====================
enum Mode {
MODE_MANUAL = 1,
MODE_LINE_FOLLOWING = 2,
MODE_OBSTACLE_AVOIDANCE = 3,
MODE_HAND_FOLLOWING = 4
};
Mode currentMode = MODE_MANUAL;
// Флаги инициализации режимов (сбрасываются при смене режима)
bool obstacleAvoidServoReady = false;
// Для followHand state machine
enum HandFollowState {INIT_START, INIT_WAIT, READY};
HandFollowState handFollowState = INIT_START;
unsigned long handFollowInitTimer = 0;
// ==================== SETUP ====================
void setup() {
Serial.begin(9600);
bluetooth.begin(9600);
ultrasonic.begin();
scanServo.attach(SERVO_PIN);
scanServo.write(90); // Центр
Serial.println("╔════════════════════════════════╗");
Serial.println("║ PHOBO - Master Mode Ready! ║");
Serial.println("╚════════════════════════════════╝");
Serial.println("Режимы:");
Serial.println("1 - Manual, 2 - Line, 3 - Obstacle, 4 - Hand");
Serial.println();
stopMotors();
}
// ==================== LOOP ====================
void loop() {
// Проверка команд Bluetooth
if (bluetooth.available()) {
char cmd = bluetooth.read();
// Фильтр HM-10 service messages
if (cmd == '\n' || cmd == '\r' || cmd < 32 || cmd > 126) {
return;
}
processCommand(cmd);
}
// Выполнение текущего режима
switch (currentMode) {
case MODE_MANUAL:
// Ничего не делаем — ждём команд
break;
case MODE_LINE_FOLLOWING:
lineFollowingMode();
break;
case MODE_OBSTACLE_AVOIDANCE:
obstacleAvoidanceMode();
break;
case MODE_HAND_FOLLOWING:
followHand();
break;
}
}
// ==================== КОМАНДЫ ====================
void processCommand(char cmd) {
// Переключение режимов (работает из любого режима)
if (cmd == '1') {
currentMode = MODE_MANUAL;
stopMotors();
scanServo.detach(); // Отключаем серво
// Сброс флагов инициализации
obstacleAvoidServoReady = false;
handFollowState = INIT_START;
Serial.println(">>> Режим: MANUAL");
return;
}
if (cmd == '2') {
currentMode = MODE_LINE_FOLLOWING;
stopMotors();
scanServo.detach(); // Отключаем серво
// Сброс флагов инициализации
obstacleAvoidServoReady = false;
handFollowState = INIT_START;
Serial.println(">>> Режим: LINE FOLLOWING");
return;
}
if (cmd == '3') {
currentMode = MODE_OBSTACLE_AVOIDANCE;
stopMotors();
// Сброс флагов (серво будет подключено в obstacleAvoidanceMode)
obstacleAvoidServoReady = false;
handFollowState = INIT_START;
Serial.println(">>> Режим: OBSTACLE AVOIDANCE");
return;
}
if (cmd == '4') {
currentMode = MODE_HAND_FOLLOWING;
stopMotors();
// Сброс флагов (серво будет подключено в followHand)
obstacleAvoidServoReady = false;
handFollowState = INIT_START;
Serial.println(">>> Режим: HAND FOLLOWING");
return;
}
// Запрос телеметрии датчиков (кнопка 9)
if (cmd == '9') {
Serial.println(F("[BT] Sensor request"));
sendSensorDataOnce();
return;
}
// Ручное управление (только в режиме Manual)
if (currentMode == MODE_MANUAL) {
switch (cmd) {
// WASD клавиши
case 'W':
moveForward();
Serial.println("▲ Вперёд");
break;
case 'w':
stopMotors();
break;
case 'S':
moveBackward();
Serial.println("▼ Назад");
break;
case 's':
stopMotors();
break;
case 'A':
turnLeft();
Serial.println("◄ Влево");
break;
case 'a':
stopMotors();
break;
case 'D':
turnRight();
Serial.println("► Вправо");
break;
case 'd':
stopMotors();
break;
// Стрелки (arrows)
case 'F':
moveForward();
Serial.println("▲ Вперёд");
break;
case 'f':
stopMotors();
break;
case 'B':
moveBackward();
Serial.println("▼ Назад");
break;
case 'b':
stopMotors();
break;
case 'L':
turnLeft();
Serial.println("◄ Влево");
break;
case 'l':
stopMotors();
break;
case 'R':
turnRight();
Serial.println("► Вправо");
break;
case 'r':
stopMotors();
break;
}
}
}
// ==================== РЕЖИМЫ РАБОТЫ ====================
// Режим 2: Следование по линии (из Проекта 9)
void lineFollowingMode() {
int left = analogRead(LINE_LEFT);
int center = analogRead(LINE_CENTER);
int right = analogRead(LINE_RIGHT);
bool onLineLeft = LEFT_INVERTED ? (left > LEFT_THRESHOLD) : (left < LEFT_THRESHOLD);
bool onLineCenter = CENTER_INVERTED ? (center > CENTER_THRESHOLD) : (center < CENTER_THRESHOLD);
bool onLineRight = RIGHT_INVERTED ? (right > RIGHT_THRESHOLD) : (right < RIGHT_THRESHOLD);
// Логика движения по линии
if (onLineCenter) {
// Едем прямо
motorLeft.setSpeed(SPEED_SEARCH);
motorRight.setSpeed(SPEED_SEARCH);
}
else if (onLineLeft) {
// Поворот влево
motorLeft.setSpeed(-SPEED_BACKWARD);
motorRight.setSpeed(SPEED_FORWARD);
}
else if (onLineRight) {
// Поворот вправо
motorLeft.setSpeed(SPEED_FORWARD);
motorRight.setSpeed(-SPEED_BACKWARD);
}
else {
// Линия потеряна — поиск
motorLeft.setSpeed(-SPEED_SEARCH);
motorRight.setSpeed(SPEED_SEARCH);
}
}
// Режим 3: Объезд препятствий (из Проекта 7)
void obstacleAvoidanceMode() {
// Инициализация серво один раз при входе в режим
if (!obstacleAvoidServoReady) {
motorLeft.stop();
motorRight.stop();
scanServo.attach(SERVO_PIN);
delay(SERVO_ATTACH_DELAY);
scanServo.write(SERVO_CENTER);
delay(300);
obstacleAvoidServoReady = true;
Serial.println(F("[AVOID] Servo initialized"));
}
float distanceCenter = ultrasonic.getDistance();
if (distanceCenter > 30) {
// Путь свободен — едем вперёд
motorLeft.setSpeed(SPEED_FORWARD);
motorRight.setSpeed(SPEED_FORWARD);
}
else {
// Препятствие — сканируем и решаем
stopMotors();
delay(200);
// Смотрим налево
scanServo.write(150);
delay(300);
float distanceLeft = ultrasonic.getDistance();
// Смотрим направо
scanServo.write(30);
delay(300);
float distanceRight = ultrasonic.getDistance();
// Возвращаем серво в центр
scanServo.write(90);
delay(300);
// Решение: куда повернуть
if (distanceLeft > distanceRight && distanceLeft > 20) {
// Поворот влево
motorLeft.setSpeed(-SPEED_TURN);
motorRight.setSpeed(SPEED_TURN);
delay(300);
}
else if (distanceRight > 20) {
// Поворот вправо
motorLeft.setSpeed(SPEED_TURN);
motorRight.setSpeed(-SPEED_TURN);
delay(300);
}
else {
// Разворот на 180°
motorLeft.setSpeed(-SPEED_TURN);
motorRight.setSpeed(SPEED_TURN);
delay(600);
}
}
}
// Режим 4: Следование за рукой (улучшенный с state machine)
void followHand() {
// Инициализация серво БЕЗ блокирующего delay()
switch (handFollowState) {
case INIT_START:
// ПОЛНОСТЬЮ отключаем моторы
motorLeft.stop();
motorRight.stop();
scanServo.attach(SERVO_PIN);
delay(SERVO_ATTACH_DELAY);
scanServo.write(SERVO_CENTER);
handFollowInitTimer = millis();
handFollowState = INIT_WAIT;
Serial.println(F("[HAND] Servo initializing..."));
return; // Выходим, продолжим в следующем цикле
case INIT_WAIT:
if (millis() - handFollowInitTimer > INIT_SERVO_WAIT) { // Ждём серво без блокировки
scanServo.detach();
handFollowState = READY;
Serial.println(F("[HAND] Servo ready, detached"));
}
return; // Продолжаем ждать
case READY:
break; // Серво готов, продолжаем работу
}
float distance = ultrasonic.getDistance();
// Отладка каждые 500мс
static unsigned long lastDebug = 0;
if (millis() - lastDebug > 500) {
Serial.print(F("Follow: D="));
Serial.print(distance);
Serial.print(F("cm"));
lastDebug = millis();
}
if (distance > 0 && distance < 200) {
if (distance < DISTANCE_TOO_CLOSE) {
Serial.println(F(" -> TOO CLOSE"));
moveBackward();
}
else if (distance > DISTANCE_TOO_FAR) {
Serial.println(F(" -> TOO FAR"));
moveForward();
}
else {
Serial.println(F(" -> PERFECT"));
stopMotors();
}
} else {
Serial.println(F(" -> OUT OF RANGE"));
stopMotors();
}
// НЕТ delay()! Цикл выполняется максимально быстро
}
// ==================== ДВИЖЕНИЕ ====================
void moveForward() {
motorLeft.setSpeed(SPEED_FORWARD);
motorRight.setSpeed(SPEED_FORWARD);
}
void moveBackward() {
motorLeft.setSpeed(-SPEED_BACKWARD);
motorRight.setSpeed(-SPEED_BACKWARD);
}
void turnLeft() {
motorLeft.setSpeed(-SPEED_TURN);
motorRight.setSpeed(SPEED_TURN);
}
void turnRight() {
motorLeft.setSpeed(SPEED_TURN);
motorRight.setSpeed(-SPEED_TURN);
}
void stopMotors() {
motorLeft.setSpeed(0);
motorRight.setSpeed(0);
}
// ==================== ТЕЛЕМЕТРИЯ ====================
void sendSensorDataOnce() {
// Читаем датчики линии (быстрые аналоговые операции)
int lineLeft = analogRead(LINE_LEFT);
int lineCenter = analogRead(LINE_CENTER);
int lineRight = analogRead(LINE_RIGHT);
// Читаем ультразвуковой датчик
float distance = ultrasonic.getDistance();
// Отправляем данные максимально компактно
bluetooth.print(F("S1:"));
bluetooth.print((int)distance);
bluetooth.print(F("\nS2:"));
bluetooth.print(lineLeft);
bluetooth.print(F("\nS3:"));
bluetooth.print(lineCenter);
bluetooth.print(F("\nS4:"));
bluetooth.println(lineRight);
// Дублируем в Serial Monitor для отладки
Serial.print(F("Sensors: D="));
Serial.print((int)distance);
Serial.print(F(" L="));
Serial.print(lineLeft);
Serial.print(F(" C="));
Serial.print(lineCenter);
Serial.print(F(" R="));
Serial.println(lineRight);
}
Альтернативный код с ИК-пультом
Важно
Расширенная версия с поддержкой IR-пульта
Этот код добавляет полную поддержку ИК-пульта (VS1838B) в дополнение к Bluetooth управлению. Вы можете использовать ЛЮБОЙ источник управления: BLE Controller или IR-пульт.
Дополнительные возможности:
✅ Двойное управление: Bluetooth + IR-пульт (любой на выбор)
✅ IR auto-stop: Автоматическая остановка через 200мс при потере сигнала
✅ Переключение режимов: Кнопки 1, 2, 3 на пульте
✅ Экстренный стоп: Кнопка OK на пульте
/*
* Проект 14: Мастер-режим - полнофункциональный робот Фобо
* Управление: ALASH Electronics (Android) ИЛИ ИК-пульт
*
* Возможности:
* - Векторное управление (8 направлений: вверх/вниз/влево/вправо + диагонали)
* - Bluetooth джойстик (плавное управление) или ИК-пульт (4 направления)
* - 3 автономных режима (линия, препятствия, следование)
* - Переключение режимов кнопками 1, 2, 3 (Bluetooth или IR)
* - Экстренный останов кнопкой Y (Bluetooth) или OK (IR)
* - Телеметрия на экран телефона
* - Передача данных датчиков (ультразвук + линия) в приложение каждые 100мс
* - Мгновенная остановка (Bluetooth) или auto-stop 200мс (IR)
*
* Формат данных датчиков (отправляется через Bluetooth):
* - S1:<distance> - Ультразвуковой датчик (расстояние в см, 0-400)
* - S2:<value> - Датчик линии слева (аналоговое значение 0-1023)
* - S3:<value> - Датчик линии центр (аналоговое значение 0-1023)
* - S4:<value> - Датчик линии справа (аналоговое значение 0-1023)
*
* Подключение (оптимизировано для избежания конфликта таймеров):
* - HM-10 Bluetooth: RX=D10, TX=D11
* - IR приёмник VS1838B: A3
* - Моторы L298N:
* Motor B (левый): IN3=D12, IN4=D8, ENB=D6 (PWM на Timer0)
* Motor A (правый): IN1=D4, IN2=D2, ENA=D5 (PWM на Timer0)
* - Servo: D9 (использует Timer1 - нет конфликта с PWM моторов)
* - RCWL-9610A: Trig=D3, Echo=D7
* - Датчики линии: Left=A0, Center=A1, Right=A2
*
* ВАЖНО: Данная конфигурация пинов исключает конфликты между таймерами Arduino.
* PWM для моторов (D5, D6) работает на Timer0, Servo (D9) работает на Timer1.
*/
#include <SoftwareSerial.h>
#include <Servo.h>
#include <AlashUltrasonic.h>
#include <AlashMotorControlLite.h>
#include <AlashIRControlRX.h>
// ==================== НАСТРОЙКИ BLUETOOTH ====================
const uint8_t BT_RX = 10;
const uint8_t BT_TX = 11;
SoftwareSerial bluetooth(BT_RX, BT_TX);
// ==================== НАСТРОЙКИ ИК-ПУЛЬТА ====================
const uint8_t IR_PIN = A3;
AlashIRControlRX irReceiver(IR_PIN);
// Коды кнопок пульта (ВАЖНО: Проверьте свои коды в Проекте 10!)
const unsigned long BTN_UP = 0xFF18E7; // ▲ Вперёд
const unsigned long BTN_DOWN = 0xFF4AB5; // ▼ Назад
const unsigned long BTN_LEFT = 0xFF10EF; // ◄ Налево
const unsigned long BTN_RIGHT = 0xFF5AA5; // ► Направо
const unsigned long BTN_OK = 0xFF38C7; // OK - Экстренный останов
const unsigned long BTN_1 = 0xFFA25D; // 1 - Режим линии
const unsigned long BTN_2 = 0xFF629D; // 2 - Режим препятствий
const unsigned long BTN_3 = 0xFFE21D; // 3 - Режим следования
const unsigned long BTN_REPEAT = 0xFFFFFFFF; // Код повтора (игнорировать)
unsigned long lastIRSignalTime = 0; // Время последнего IR сигнала (для auto-stop)
const int IR_TIMEOUT = 200; // Таймаут для IR (мс) - робот останавливается если сигнал не приходит
// ==================== НАСТРОЙКИ МОТОРОВ ====================
// Левый мотор (Motor B: D6, D8, D12)
const uint8_t MOTOR_L_IN3 = 8;
const uint8_t MOTOR_L_IN4 = 12;
const uint8_t MOTOR_L_ENB = 6;
// Правый мотор (Motor A: D2, D4, D5)
const uint8_t MOTOR_R_IN1 = 4;
const uint8_t MOTOR_R_IN2 = 2;
const uint8_t MOTOR_R_ENA = 5;
AlashMotorControlLite motorLeft(DIR_DIR_PWM, MOTOR_L_IN3, MOTOR_L_IN4, MOTOR_L_ENB);
AlashMotorControlLite motorRight(DIR_DIR_PWM, MOTOR_R_IN1, MOTOR_R_IN2, MOTOR_R_ENA);
// Константы скорости для ручного управления
const int SPEED_NORMAL = 100;
const int SPEED_SLOW = 60;
const int SPEED_TURN = 65; // Скорость поворота на месте (оптимально для резких поворотов)
// Константы скорости для АГРЕССИВНОГО следования по линии (Проект 9)
// ВАЖНО: Эти значения оптимизированы для резких поворотов с реверсом!
const int FORWARD_SPEED = 70; // Скорость прямо (умеренная для стабильности)
const int TURN_SPEED = 80; // Скорость внешнего колеса при повороте
const int REVERSE_SPEED = -60; // РЕВЕРС внутреннего колеса (минус = назад!)
// ==================== НАСТРОЙКИ ДАТЧИКОВ ====================
// Ультразвуковой датчик
const uint8_t TRIG_PIN = 3;
const uint8_t ECHO_PIN = 7;
AlashUltrasonic ultrasonic(TRIG_PIN, ECHO_PIN);
// Сервопривод для сканирования
const uint8_t SERVO_PIN = 9;
const int SERVO_CENTER = 100; // Калибровка центра серво (90 = стандарт, настройте под ваш серво)
Servo scanServo;
// Датчики линии (аналоговые)
const uint8_t LINE_LEFT = A0;
const uint8_t LINE_CENTER = A1;
const uint8_t LINE_RIGHT = A2;
// КАЛИБРОВКА ДАТЧИКОВ ЛИНИИ (инвертированные датчики!)
const int LEFT_WHITE = 34, LEFT_BLACK = 53;
const int CENTER_WHITE = 34, CENTER_BLACK = 53;
const int RIGHT_WHITE = 36, RIGHT_BLACK = 53;
// Индивидуальные пороги для каждого датчика
const int LEFT_THRESHOLD = (LEFT_WHITE + LEFT_BLACK) / 2; // 33
const int CENTER_THRESHOLD = (CENTER_WHITE + CENTER_BLACK) / 2; // 35
const int RIGHT_THRESHOLD = (RIGHT_WHITE + RIGHT_BLACK) / 2; // 31
// Определяем, инвертированные ли датчики (высокие значения = черное)
const bool INVERTED = (LEFT_BLACK > LEFT_WHITE); // true для этих датчиков
// ==================== РЕЖИМЫ РАБОТЫ ====================
enum Mode {
MODE_MANUAL, // 0: Ручное управление джойстиком
MODE_LINE_FOLLOWING, // 1: Следование по линии
MODE_OBSTACLE_AVOIDANCE, // 2: Объезд препятствий
MODE_HAND_FOLLOWING // 3: Следование за рукой
};
Mode currentMode = MODE_MANUAL;
// ==================== ПАРАМЕТРЫ ДЛЯ АВТОНОМНЫХ РЕЖИМОВ ====================
// Следование за рукой
const int DISTANCE_TOO_CLOSE = 10;
const int DISTANCE_PERFECT = 20;
const int DISTANCE_TOO_FAR = 40;
// Объезд препятствий
const int OBSTACLE_DISTANCE = 30; // Порог обнаружения препятствия (см)
// ==================== КОНСТАНТЫ ЗАДЕРЖЕК ====================
// Задержки для управления питанием моторов и серво
const int MOTOR_STOP_DELAY = 50; // Задержка после остановки моторов (мс)
const int MOTOR_PIN_SWITCH_DELAY = 50; // Задержка при переключении режима пинов (мс)
const int SERVO_ATTACH_DELAY = 20; // Задержка после attach серво (мс)
const int SERVO_STABILIZE_DELAY = 300; // Задержка стабилизации серво после поворота (мс)
const int SERVO_DETACH_DELAY = 20; // Задержка после detach серво (мс)
const int INIT_SERVO_WAIT = 500; // Задержка инициализации серво в автономных режимах (мс)
const int SETUP_INIT_DELAY = 500; // Задержка в конце setup() (мс)
// ==================== ПАРАМЕТРЫ ТЕЛЕМЕТРИИ ====================
// Телеметрия отправляется по запросу (кнопка 9) - один раз при нажатии
// ==================== SETUP ====================
void setup() {
Serial.begin(9600);
bluetooth.begin(9600);
irReceiver.begin();
ultrasonic.begin();
// Настройка пинов моторов как OUTPUT
pinMode(LINE_LEFT, INPUT);
pinMode(LINE_CENTER, INPUT);
pinMode(LINE_RIGHT, INPUT);
Serial.println(F("PHOBO v2.0 - BT+IR"));
Serial.println(F("Ready!"));
bluetooth.println(F("Phobo Ready!"));
bluetooth.println(F("BT or IR control"));
stopMotors();
delay(SETUP_INIT_DELAY);
}
// ==================== MAIN LOOP ====================
void loop() {
// === ЧИТАЕМ ВСЕ КОМАНДЫ ИЗ БУФЕРА BLUETOOTH ===
// Важно: читаем все доступные команды за один цикл, чтобы буфер не переполнялся
while (bluetooth.available()) {
char cmd = bluetooth.read();
processCommand(cmd);
}
// === ПРОВЕРЯЕМ КОМАНДЫ ОТ ИК-ПУЛЬТА ===
// Параметр true разрешает обработку кодов повтора (удержание кнопки)
if (irReceiver.check(true)) {
unsigned long irCode = irReceiver.data;
processIRCommand(irCode);
}
// === TIMEOUT ДЛЯ IR УПРАВЛЕНИЯ ===
// Автоматически останавливаем робот, если IR сигнал не приходит более IR_TIMEOUT мс
// (Bluetooth не нуждается в timeout, т.к. отправляет команды отпускания)
if (currentMode == MODE_MANUAL && lastIRSignalTime > 0) {
if (millis() - lastIRSignalTime > IR_TIMEOUT) {
stopMotors();
lastIRSignalTime = 0; // Сбрасываем таймер
}
}
// === ВЫПОЛНЯЕМ ДЕЙСТВИЯ В ЗАВИСИМОСТИ ОТ РЕЖИМА ===
switch (currentMode) {
case MODE_MANUAL:
// В ручном режиме робот управляется только джойстиком
// Команды уже обработаны в processCommand()
break;
case MODE_LINE_FOLLOWING:
followLine();
break;
case MODE_OBSTACLE_AVOIDANCE:
avoidObstacles();
break;
case MODE_HAND_FOLLOWING:
followHand();
break;
}
// === ОТПРАВКА ДАННЫХ ДАТЧИКОВ ПО ЗАПРОСУ (кнопка 9) ===
// Данные отправляются только при нажатии кнопки 9, не автоматически
}
// ==================== ОБРАБОТКА КОМАНД BLUETOOTH ====================
void processCommand(char cmd) {
// === ПЕРЕКЛЮЧЕНИЕ РЕЖИМОВ (кнопки 1, 2, 3) ===
if (cmd == '1') {
resetAutonomousModes(); // Сброс состояния перед входом в автономный режим
currentMode = MODE_LINE_FOLLOWING;
stopMotors();
Serial.println(F("[BT] Mode: LINE FOLLOW"));
bluetooth.println(F("Line Mode"));
return;
}
if (cmd == '2') {
resetAutonomousModes(); // Сброс состояния перед входом в автономный режим
currentMode = MODE_OBSTACLE_AVOIDANCE;
stopMotors();
Serial.println(F("[BT] Mode: AVOID"));
bluetooth.println(F("Avoid Mode"));
return;
}
if (cmd == '3') {
resetAutonomousModes(); // Сброс состояния перед входом в автономный режим
currentMode = MODE_HAND_FOLLOWING;
stopMotors();
Serial.println(F("[BT] Mode: HAND FOLLOW"));
bluetooth.println(F("Follow Mode"));
return;
}
// === ЭКСТРЕННЫЙ ОСТАНОВ (кнопка 4) ===
if (cmd == '4') {
resetAutonomousModes(); // Полная очистка при переходе в ручной режим
currentMode = MODE_MANUAL;
stopMotors();
Serial.println(F("[BT] E-STOP -> MANUAL"));
bluetooth.println(F("STOP!"));
return;
}
// === ЗАПРОС ТЕЛЕМЕТРИИ (кнопка 9) - ОДИН РАЗ ===
if (cmd == '9') {
Serial.println(F("[BT] Sensor request"));
sendSensorDataOnce(); // Отправляем данные один раз
return;
}
// === РУЧНОЕ УПРАВЛЕНИЕ (джойстик) ===
// Работает ТОЛЬКО в ручном режиме
if (currentMode == MODE_MANUAL) {
switch (cmd) {
// === ОСНОВНЫЕ НАПРАВЛЕНИЯ ===
case 'F':
moveForward();
Serial.println(F("[BT] FWD"));
break;
case 'f':
stopMotors();
Serial.println(F("[BT] STOP"));
break;
case 'B':
moveBackward();
Serial.println(F("[BT] BACK"));
break;
case 'b':
stopMotors();
Serial.println(F("[BT] STOP"));
break;
case 'L':
turnLeft();
Serial.println(F("[BT] LEFT"));
break;
case 'l':
stopMotors();
Serial.println(F("[BT] STOP"));
break;
case 'R':
turnRight();
Serial.println(F("[BT] RIGHT"));
break;
case 'r':
stopMotors();
Serial.println(F("[BT] STOP"));
break;
// === ДИАГОНАЛЬНЫЕ НАПРАВЛЕНИЯ (векторное управление) ===
case 'Y': // Вперёд-Влево (Up-Left)
moveForwardLeft();
Serial.println(F("[BT] FWD-LEFT"));
break;
case 'y':
stopMotors();
Serial.println(F("[BT] STOP"));
break;
case 'U': // Вперёд-Вправо (Up-Right)
moveForwardRight();
Serial.println(F("[BT] FWD-RIGHT"));
break;
case 'u':
stopMotors();
Serial.println(F("[BT] STOP"));
break;
case 'H': // Назад-Влево (Down-Left)
moveBackwardLeft();
Serial.println(F("[BT] BACK-LEFT"));
break;
case 'h':
stopMotors();
Serial.println(F("[BT] STOP"));
break;
case 'J': // Назад-Вправо (Down-Right)
moveBackwardRight();
Serial.println(F("[BT] BACK-RIGHT"));
break;
case 'j':
stopMotors();
Serial.println(F("[BT] STOP"));
break;
}
}
}
// ==================== ОБРАБОТКА КОМАНД ИК-ПУЛЬТА ====================
void processIRCommand(unsigned long code) {
// Логирование только для отладки (можно закомментировать для экономии памяти)
// Serial.print(F("[IR] Code: 0x"));
// Serial.println(code, HEX);
// === ПЕРЕКЛЮЧЕНИЕ РЕЖИМОВ (кнопки 1, 2, 3) ===
if (code == BTN_1) {
resetAutonomousModes(); // Сброс состояния перед входом в автономный режим
currentMode = MODE_LINE_FOLLOWING;
stopMotors();
Serial.println(F("[IR] Mode: LINE FOLLOW"));
bluetooth.println(F("Line(IR)"));
return;
}
if (code == BTN_2) {
resetAutonomousModes(); // Сброс состояния перед входом в автономный режим
currentMode = MODE_OBSTACLE_AVOIDANCE;
stopMotors();
Serial.println(F("[IR] Mode: AVOID"));
bluetooth.println(F("Avoid(IR)"));
return;
}
if (code == BTN_3) {
resetAutonomousModes(); // Сброс состояния перед входом в автономный режим
currentMode = MODE_HAND_FOLLOWING;
stopMotors();
Serial.println(F("[IR] Mode: HAND FOLLOW"));
bluetooth.println(F("Follow(IR)"));
return;
}
// === ЭКСТРЕННЫЙ ОСТАНОВ (кнопка OK) ===
if (code == BTN_OK) {
resetAutonomousModes(); // Полная очистка при переходе в ручной режим
currentMode = MODE_MANUAL;
stopMotors();
Serial.println(F("[IR] E-STOP -> MANUAL"));
bluetooth.println(F("STOP(IR)"));
return;
}
// === РУЧНОЕ УПРАВЛЕНИЕ (стрелки) ===
// Работает ТОЛЬКО в ручном режиме
if (currentMode == MODE_MANUAL) {
// Обновляем время последнего IR сигнала (для auto-stop)
lastIRSignalTime = millis();
switch (code) {
case BTN_UP:
moveForward();
Serial.println(F("[IR] FWD"));
break;
case BTN_DOWN:
moveBackward();
Serial.println(F("[IR] BACK"));
break;
case BTN_LEFT:
turnLeft();
Serial.println(F("[IR] LEFT"));
break;
case BTN_RIGHT:
turnRight();
Serial.println(F("[IR] RIGHT"));
break;
}
}
}
// ==================== ФУНКЦИЯ СБРОСА СОСТОЯНИЯ АВТОНОМНЫХ РЕЖИМОВ ====================
void resetAutonomousModes() {
// Эта функция НЕ МОЖЕТ напрямую сбросить static переменные внутри функций,
// но гарантирует, что моторы остановлены и серво отключён
stopMotors();
if (scanServo.attached()) {
scanServo.detach();
}
}
// ==================== ФУНКЦИИ ДВИЖЕНИЯ ====================
void moveForward() {
motorLeft.setSpeed(SPEED_NORMAL);
motorRight.setSpeed(SPEED_NORMAL);
}
void moveBackward() {
motorLeft.setSpeed(-SPEED_NORMAL);
motorRight.setSpeed(-SPEED_NORMAL);
}
void turnLeft() {
motorLeft.setSpeed(-SPEED_TURN);
motorRight.setSpeed(SPEED_TURN);
}
void turnRight() {
motorLeft.setSpeed(SPEED_TURN);
motorRight.setSpeed(-SPEED_TURN);
}
void stopMotors() {
motorLeft.setSpeed(0);
motorRight.setSpeed(0);
}
// === ДИАГОНАЛЬНЫЕ ДВИЖЕНИЯ (дифференциальное рулевое управление) ===
void moveForwardLeft() {
// Вперёд-влево: правое колесо вперёд, левое СТОП
motorLeft.setSpeed(0); // Левое стоит на месте
motorRight.setSpeed(-SPEED_NORMAL); // Правое вперёд
}
void moveForwardRight() {
// Вперёд-вправо: левое колесо вперёд, правое СТОП
motorLeft.setSpeed(-SPEED_NORMAL); // Левое вперёд
motorRight.setSpeed(0); // Правое стоит на месте
}
void moveBackwardLeft() {
// Назад-влево: правое колесо назад, левое СТОП
motorLeft.setSpeed(0); // Левое стоит на месте
motorRight.setSpeed(SPEED_NORMAL); // Правое назад
}
void moveBackwardRight() {
// Назад-вправо: левое колесо назад, правое СТОП
motorLeft.setSpeed(SPEED_NORMAL); // Левое назад
motorRight.setSpeed(0); // Правое стоит на месте
}
// ==================== ФУНКЦИЯ ПРОВЕРКИ ЛИНИИ ====================
// Проверяет, находится ли датчик над чёрной линией
bool isOnLine(int value, int threshold) {
if (INVERTED) {
return value > threshold; // Инвертированные: больше = чёрное
} else {
return value < threshold; // Обычные: меньше = чёрное
}
}
// ==================== РЕЖИМ 1: СЛЕДОВАНИЕ ПО ЛИНИИ (АГРЕССИВНЫЙ) ====================
void followLine() {
// Чтение аналоговых значений (0-1023)
int leftValue = analogRead(LINE_LEFT);
int centerValue = analogRead(LINE_CENTER);
int rightValue = analogRead(LINE_RIGHT);
// Определяем, на линии ли каждый датчик
bool L = isOnLine(leftValue, LEFT_THRESHOLD);
bool C = isOnLine(centerValue, CENTER_THRESHOLD);
bool R = isOnLine(rightValue, RIGHT_THRESHOLD);
// ========== АГРЕССИВНАЯ ЛОГИКА С РЕВЕРСОМ ==========
// Случай 1: Центр на линии (0-1-0) → ПРЯМО
if (!L && C && !R) {
motorLeft.setSpeed(FORWARD_SPEED);
motorRight.setSpeed(FORWARD_SPEED);
}
// Случай 2: ЛЮБОЙ левый датчик видит линию → РЕЗКИЙ поворот налево с РЕВЕРСОМ
else if (L) {
// Левое колесо НАЗАД, правое колесо ВПЕРЁД
motorLeft.setSpeed(REVERSE_SPEED); // Реверс (отрицательное значение)
motorRight.setSpeed(TURN_SPEED); // Вперёд быстро
}
// Случай 3: ЛЮБОЙ правый датчик видит линию → РЕЗКИЙ поворот направо с РЕВЕРСОМ
else if (R) {
// Левое колесо ВПЕРЁД, правое колесо НАЗАД
motorLeft.setSpeed(TURN_SPEED); // Вперёд быстро
motorRight.setSpeed(REVERSE_SPEED); // Реверс (отрицательное значение)
}
// Случай 4: Линия потеряна (0-0-0) → медленно вперёд (поиск)
else if (!L && !C && !R) {
motorLeft.setSpeed(FORWARD_SPEED / 2);
motorRight.setSpeed(FORWARD_SPEED / 2);
}
// НЕТ delay()! Цикл работает максимально быстро для мгновенной реакции
}
// ==================== РЕЖИМ 2: ОБЪЕЗД ПРЕПЯТСТВИЙ ====================
void avoidObstacles() {
// State machine для обработки препятствий БЕЗ delay()
static enum {INIT_START, INIT_WAIT, MOVING, STOP_WAIT, SCAN_LEFT, SCAN_WAIT1, SCAN_RIGHT, SCAN_WAIT2, SCAN_CENTER, TURNING, TURN_WAIT} state = INIT_START;
static unsigned long stateTimer = 0;
static int distLeft = 0, distRight = 0;
static unsigned long lastDebug = 0;
// === ИНИЦИАЛИЗАЦИЯ: ЦЕНТРИРОВАНИЕ СЕРВО ПРИ ВХОДЕ В РЕЖИМ ===
switch (state) {
case INIT_START:
Serial.println(F("[AVOID] Centering servo..."));
// ПОЛНОСТЬЮ отключаем моторы
motorLeft.stop();
motorRight.stop();
scanServo.attach(SERVO_PIN);
delay(SERVO_ATTACH_DELAY);
scanServo.write(SERVO_CENTER); // Серво смотрит вперёд
stateTimer = millis();
state = INIT_WAIT;
return;
case INIT_WAIT:
if (millis() - stateTimer > INIT_SERVO_WAIT) {
scanServo.detach();
// Восстанавливаем ВСЕ пины моторов
Serial.println(F("[AVOID] Ready!"));
state = MOVING;
}
return;
}
float distance = ultrasonic.getDistance();
// Отладка каждые 500мс
if (millis() - lastDebug > 500) {
Serial.print(F("Avoid: D="));
Serial.print(distance);
Serial.println(F("cm"));
lastDebug = millis();
}
// === STATE MACHINE ===
switch (state) {
case MOVING:
// Едем вперёд, проверяем препятствия
if (distance < OBSTACLE_DISTANCE && distance > 0) {
Serial.println(F("-> OBSTACLE!"));
stopMotors();
stateTimer = millis();
state = STOP_WAIT;
} else {
moveForward();
}
break;
case STOP_WAIT:
// Ждём 300мс после остановки
if (millis() - stateTimer > 300) {
Serial.println(F("-> Scanning..."));
state = SCAN_LEFT;
}
break;
case SCAN_LEFT:
// Сканируем налево
distLeft = scanDirection(150);
Serial.print(F("Left="));
Serial.print(distLeft);
Serial.print(F("cm"));
stateTimer = millis();
state = SCAN_WAIT1;
break;
case SCAN_WAIT1:
// Ждём 200мс между сканированиями
if (millis() - stateTimer > 200) {
state = SCAN_RIGHT;
}
break;
case SCAN_RIGHT:
// Сканируем направо
distRight = scanDirection(30);
Serial.print(F(" Right="));
Serial.print(distRight);
Serial.println(F("cm"));
stateTimer = millis();
state = SCAN_WAIT2;
break;
case SCAN_WAIT2:
// Ждём 200мс перед возвратом серво в центр
if (millis() - stateTimer > 200) {
state = SCAN_CENTER;
}
break;
case SCAN_CENTER:
// Возвращаем серво в центр
scanDirection(SERVO_CENTER);
// Выбираем направление поворота
if (distLeft > distRight) {
Serial.println(F("-> Turn LEFT"));
turnLeft();
} else {
Serial.println(F("-> Turn RIGHT"));
turnRight();
}
stateTimer = millis();
state = TURNING;
break;
case TURNING:
// Поворачиваем 500мс
if (millis() - stateTimer > 500) {
stopMotors();
stateTimer = millis();
state = TURN_WAIT;
}
break;
case TURN_WAIT:
// Ждём 200мс после поворота
if (millis() - stateTimer > 200) {
state = MOVING; // Возвращаемся к движению
}
break;
}
}
// Функция сканирования (с ПОЛНЫМ отключением питания моторов)
int scanDirection(int angle) {
// === КРИТИЧЕСКИ ВАЖНО: ПОЛНОСТЬЮ ОТКЛЮЧАЕМ МОТОРЫ ===
// Останавливаем моторы через библиотеку
motorLeft.stop();
motorRight.stop();
delay(MOTOR_STOP_DELAY);
delay(MOTOR_PIN_SWITCH_DELAY);
// === БЫСТРЫЙ ПОВОРОТ БЕЗ ПЛАВНОСТИ ===
scanServo.attach(SERVO_PIN);
delay(SERVO_ATTACH_DELAY);
scanServo.write(angle); // Прямой поворот на нужный угол
delay(SERVO_STABILIZE_DELAY);
float dist = ultrasonic.getDistance();
scanServo.detach(); // Отключаем серво
delay(SERVO_DETACH_DELAY);
// === ВОССТАНАВЛИВАЕМ ВСЕ ПИНЫ МОТОРОВ ===
return (int)dist;
}
// ==================== РЕЖИМ 3: СЛЕДОВАНИЕ ЗА РУКОЙ ====================
void followHand() {
// Инициализация серво БЕЗ блокирующего delay()
static enum {INIT_START, INIT_WAIT, READY} initState = INIT_START;
static unsigned long initTimer = 0;
switch (initState) {
case INIT_START:
// ПОЛНОСТЬЮ отключаем моторы
motorLeft.stop();
motorRight.stop();
scanServo.attach(SERVO_PIN);
delay(SERVO_ATTACH_DELAY);
scanServo.write(SERVO_CENTER);
initTimer = millis();
initState = INIT_WAIT;
return; // Выходим, продолжим в следующем цикле
case INIT_WAIT:
if (millis() - initTimer > INIT_SERVO_WAIT) { // Ждём серво без блокировки
scanServo.detach();
// Восстанавливаем ВСЕ пины моторов
initState = READY;
}
return; // Продолжаем ждать
case READY:
break; // Серво готов, продолжаем работу
}
float distance = ultrasonic.getDistance();
// Отладка каждые 500мс
static unsigned long lastDebug = 0;
if (millis() - lastDebug > 500) {
Serial.print(F("Follow: D="));
Serial.print(distance);
Serial.print(F("cm"));
lastDebug = millis();
}
if (distance > 0 && distance < 200) {
if (distance < DISTANCE_TOO_CLOSE) {
Serial.println(F(" -> TOO CLOSE"));
moveBackward();
}
else if (distance > DISTANCE_TOO_FAR) {
Serial.println(F(" -> TOO FAR"));
moveForward();
}
else {
Serial.println(F(" -> PERFECT"));
stopMotors();
}
} else {
Serial.println(F(" -> OUT OF RANGE"));
stopMotors();
}
// НЕТ delay()! Цикл выполняется максимально быстро
}
// ==================== ОТПРАВКА ДАННЫХ ДАТЧИКОВ ОДИН РАЗ ====================
void sendSensorDataOnce() {
// Читаем датчики линии (быстрые аналоговые операции)
int lineLeft = analogRead(LINE_LEFT);
int lineCenter = analogRead(LINE_CENTER);
int lineRight = analogRead(LINE_RIGHT);
// Читаем ультразвуковой датчик
float distance = ultrasonic.getDistance();
// Отправляем данные максимально компактно
bluetooth.print(F("S1:"));
bluetooth.print((int)distance);
bluetooth.print(F("\nS2:"));
bluetooth.print(lineLeft);
bluetooth.print(F("\nS3:"));
bluetooth.print(lineCenter);
bluetooth.print(F("\nS4:"));
bluetooth.println(lineRight);
// Дублируем в Serial Monitor для отладки
Serial.print(F("Sensors: D="));
Serial.print((int)distance);
Serial.print(F(" L="));
Serial.print(lineLeft);
Serial.print(F(" C="));
Serial.print(lineCenter);
Serial.print(F(" R="));
Serial.println(lineRight);
}
Примечание
Отличия от основного кода:
✅ Добавлена библиотека
AlashIRControlRX.h✅ Функция
processIRCommand()для обработки IR-пульта✅ IR auto-stop timeout (200мс)
✅ Поддержка кнопок 1, 2, 3, OK на пульте
✅ Код повтора (0xFFFFFFFF) для удержания кнопок
Загрузка и тестирование
Загрузка программы
Важно: Отключите батареи перед загрузкой!
Подключите Arduino к компьютеру (USB)
Выберите Arduino Uno и правильный Порт
Нажмите Загрузить (Ctrl+U)
Откройте Serial Monitor (9600 baud)
Вы должны увидеть:
PHOBO v2.0 - BT+IR
Ready!
Phobo Ready!
BT or IR control
Тестирование режимов
Тест 1: Режим Manual (старт)
Отключите USB, вставьте батареи
Откройте BLE Controller в браузере → режим RC Car
Подключитесь к BT05
Робот стартует в Manual mode
Проверьте управление: стрелки F/B/L/R работают
Тест 2: Line Following (кнопка 1)
Подготовьте поле с чёрной линией (из Проекта 9)
Поставьте робота на линию
Нажмите кнопку 1 → Serial Monitor: «[BT] Mode: LINE FOLLOW»
Робот должен начать ехать по линии автоматически
Команды F/B/L/R игнорируются!
Тест 3: Obstacle Avoidance (кнопка 2)
Подготовьте пространство 2x2м с препятствиями
Нажмите кнопку 2 → Serial Monitor: «[BT] Mode: AVOID»
Робот должен ехать вперёд, сканировать препятствия и объезжать их
Проверьте: серво поворачивается влево/вправо при препятствии
Тест 4: Hand Following (кнопка 3)
Нажмите кнопку 3 → Serial Monitor: «[BT] Mode: HAND FOLLOW»
Поднесите руку на 15-20 см перед роботом
Робот должен ехать вперёд, останавливаясь на 10-15 см
Отдалите руку → робот едет вперёд
Приблизьте → робот едет назад
Тест 5: Emergency Stop (кнопка 4)
Запустите режим 1 (Line Following)
Дождитесь, пока робот начнёт двигаться
Нажмите кнопку 4 → Serial Monitor: «[BT] E-STOP -> MANUAL»
Робот останавливается, переключается в Manual
Попробуйте стрелки F/B/L/R → робот управляется вручную
Нажмите 2 → робот переключается в Obstacle Avoidance
Тест 6: Телеметрия датчиков (кнопка 9)
Откройте BLE Controller в браузере, режим RC Car
Подключитесь к роботу (BT05)
Нажмите кнопку 9 (в центральной панели)
В полях над кнопками появятся значения:
Ultrasonic: 25 (см)
Line Left: 28
Line Center: 30
Line Right: 26
Проверьте значения датчиков в реальном времени:
Приблизьте руку к ультразвуку → нажмите 9 → значение Ultrasonic уменьшится
Поднесите датчики линии к чёрной ленте → нажмите 9 → значения Line Left/Center/Right изменятся
(Опционально) В Serial Monitor появится:
[BT] Sensor request Sensors: D=25 L=28 C=30 R=26
Совет
Для непрерывного мониторинга нажимайте кнопку 9 многократно. Значения обновляются каждый раз при нажатии!
Поиск неисправностей
Робот не реагирует на кнопки 1-4
Проверьте: - Подключение Bluetooth (зелёная точка в BLE Controller) - Serial Monitor показывает команды (1/2/3/4) - Правильно ли настроены кнопки в BLE Controller (Settings → Control Panel)
Решение: - В BLE Controller: Settings → Control Panel Settings → убедитесь, что кнопки 1-4 отправляют символы „1“, „2“, „3“, „4“
Режим Line Following не работает
Проверьте: - Датчики линии откалиброваны (Проект 8) - Значения WHITE/BLACK/THRESHOLD в коде соответствуют вашим датчикам - Поле с чёрной линией контрастное (белый фон, чёрная лента)
Решение: - Откройте Проект 8, заново откалибруйте датчики - Обновите константы LEFT_WHITE, LEFT_BLACK, etc. в коде
Режим Obstacle Avoidance не объезжает
Проверьте: - Ультразвуковой датчик работает (Проект 5) - Серво поворачивается влево/вправо при препятствии - Препятствие не слишком низкое/высокое (оптимально 10-30 см высота)
Решение: - Проверьте провода серво (D9) и ультразвука (D3, D7) - Убедитесь, что SPEED_TURN >= 65 (минимум для поворота)
Режим Hand Following дёргается
Проверьте: - Ультразвуковой датчик чистый (без пыли на мембранах) - Робот стоит на ровной поверхности - Расстояние до руки 10-30 см
Решение: - Увеличьте зоны dead zone (10-15 см → 8-18 см) - Уменьшите SPEED_FORWARD до 60 (более плавное движение)
В автономных режимах робот реагирует на F/B/L/R
Причина: Код обрабатывает команды движения вне проверки if (currentMode == MODE_MANUAL)
Решение:
Убедитесь, что в processCommand() обработка F/B/L/R находится внутри блока:
if (currentMode == MODE_MANUAL) {
// Обработка команд движения F/B/L/R
}
Что дальше?
🎉 Поздравляем! Вы создали полнофункционального робота с 4 режимами работы!
Что вы освоили:
✅ Интеграция всех датчиков и автономных режимов
✅ Система переключения режимов (конечный автомат)
✅ Bluetooth управление роботом со смартфона/компьютера
✅ Комбинация ручного и автономного управления
Идеи для улучшений:
💡 Добавьте режим 5: Комбинированный режим — линия + препятствия (едет по линии, но объезжает препятствия на пути)
💡 Индикация режима: Подключите RGB LED (Проект Starter Kit) и показывайте текущий режим цветом (Manual=белый, Line=синий, Obstacle=красный, Hand=зелёный)
💡 Звуковые сигналы: При переключении режимов проигрывайте разные мелодии через пассивный buzzer
💡 ИК-пульт: Добавьте управление с ИК-пульта (кнопки 1-4) как альтернативу Bluetooth (Проекты 10-11)
💡 Автономная работа: Добавьте таймер — робот сам переключает режимы каждые 30 секунд (демонстрационный режим)
Что дальше в обучении:
📚 Изучите ESP32 (WiFi, Bluetooth, камера) — более мощная платформа для роботов
🤖 Машинное обучение: Научите робота распознавать объекты с помощью TensorFlow Lite
📡 IoT интеграция: Подключите робота к облаку (MQTT, Firebase)
Спасибо за обучение с Robot Phobo! 🤖
До встречи в новых проектах! 🚀