https://pastein.ru/t/9Za
Загрузка данных
# ==============================================================================
# ФАЙЛ: main.py
# ОПИСАНИЕ: Главный скрипт управления роботом LOIK.
# ВЕРСИЯ: 3.0 (Ideal State Machine Edition)
# ==============================================================================
# ==============================================================================
# БЛОК 1: ИМПОРТ БИБЛИОТЕК И НАСТРОЙКА ОКРУЖЕНИЯ
# ==============================================================================
# Импортируем стандартные библиотеки
import random
import time
# Импортируем модули для работы с железом
from machine import Pin, SPI, PWM, time_pulse_us
import bluetooth
from ble_simple_peripheral import BLESimplePeripheral
import dht
# --- Импорт библиотеки для работы с текстом на дисплее ---
# В случае отсутствия библиотеки, код не упадет, а просто не будет выводить текст
try:
from ili9341 import Display, color565
TEXT_LIB_AVAILABLE = True
except ImportError:
TEXT_LIB_AVAILABLE = False
print("ПРЕДУПРЕЖДЕНИЕ: Библиотека ili9341.py не найдена. Текст выводиться не будет.")
# ==============================================================================
# БЛОК 2: КЛАСС ДРАЙВЕРА ДИСПЛЕЯ (ILI9341Direct)
# Этот класс отвечает за быструю отрисовку анимаций и очистку экрана.
# ==============================================================================
class DisplayDriver:
"""
Драйвер для дисплея ILI9341.
Реализует прямую работу с регистрами через SPI для максимальной скорости.
"""
def __init__(self, spi, cs_pin, dc_pin, rst_pin):
"""
Конструктор класса.
Принимает шину SPI и номера пинов управления (CS, DC, RST).
"""
# Сохраняем шину SPI для дальнейшего использования
self.spi = spi
# Инициализируем пины управления как выходы
self.cs = Pin(cs_pin, Pin.OUT)
self.dc = Pin(dc_pin, Pin.OUT)
self.rst = Pin(rst_pin, Pin.OUT)
# Выполняем аппаратный сброс дисплея
self.reset()
# Выполняем инициализацию базовыми командами
self.init_display()
def reset(self):
"""
Аппаратный сброс дисплея.
Подаем низкий уровень на RST, ждем, затем высокий.
"""
self.rst.value(0)
time.sleep_ms(50)
self.rst.value(1)
time.sleep_ms(100)
def write_cmd(self, cmd):
"""
Отправка команды в дисплей.
DC = 0 (команда), CS = 0 (выбор чипа).
"""
self.dc.value(0)
self.cs.value(0)
self.spi.write(bytearray([cmd]))
self.cs.value(1)
def write_data(self, data):
"""
Отправка данных в дисплей.
DC = 1 (данные), CS = 0 (выбор чипа).
"""
self.dc.value(1)
self.cs.value(0)
self.spi.write(data)
self.cs.value(1)
def init_display(self):
"""
Инициализация дисплея базовыми командами.
Выполняет программный сброс, выход из спящего режима,
установку формата цвета и включение дисплея.
"""
self.write_cmd(0x01) # Программный сброс
time.sleep_ms(100)
self.write_cmd(0x11) # Выход из спящего режима
time.sleep_ms(120)
self.write_cmd(0x3A) # Установка формата цвета (16 бит)
self.write_data(b'\x55')
self.write_cmd(0x36) # Настройка ориентации (альбомная)
self.write_data(b'\x28')
self.write_cmd(0x29) # Включение дисплея
time.sleep_ms(50)
def set_window(self, x0, y0, x1, y1):
"""
Установка области отрисовки (окна).
Принимает координаты левого верхнего и правого нижнего углов.
"""
# Команда установки колонок (X)
self.write_cmd(0x2A)
self.write_data(bytes([(x0 >> 8) & 0xFF, x0 & 0xFF, (x1 >> 8) & 0xFF, x1 & 0xFF]))
# Команда установки строк (Y)
self.write_cmd(0x2B)
self.write_data(bytes([(y0 >> 8) & 0xFF, y0 & 0xFF, (y1 >> 8) & 0xFF, y1 & 0xFF]))
# Команда начала записи памяти
self.write_cmd(0x2C)
def clear(self, color=0x0000):
"""
Полная очистка экрана заданным цветом.
По умолчанию - черный.
"""
# Устанавливаем окно на весь экран
self.set_window(0, 0, 319, 239)
# Создаем буфер одной строки (320 пикселей по 2 байта)
row_buf = color.to_bytes(2, 'big') * 320
# Отправляем буфер 240 раз (по количеству строк)
for _ in range(240):
self.write_data(row_buf)
# ==============================================================================
# БЛОК 3: ДЕКОДЕР АНИМАЦИЙ (VIPER)
# Переводит 1-битный формат в 16-битный RGB565 на лету.
# ==============================================================================
@micropython.viper
def decode_chunk_normal(raw_ptr: ptr8, rgb_ptr: ptr8, num_bytes: int, fg_h: int, fg_l: int, bg_h: int, bg_l: int):
"""
Функция декодирования, ускоренная через Viper.
Принимает указатели на сырые данные и буфер вывода.
"""
out_idx = 0
for i in range(num_bytes):
b = raw_ptr[i]
for bit in range(7, -1, -1):
if (b >> bit) & 1:
rgb_ptr[out_idx] = fg_h
rgb_ptr[out_idx + 1] = fg_l
else:
rgb_ptr[out_idx] = bg_h
rgb_ptr[out_idx + 1] = bg_l
out_idx += 2
# ==============================================================================
# БЛОК 4: ФУНКЦИЯ ПРОИГРЫВАНИЯ АНИМАЦИИ
# Читает файл .bin и отрисовывает его на экране.
# ==============================================================================
def play_animation(display, filename, width=240, height=240, fps=30, fg_color=0xFFFF, bg_color=0x0000):
"""
Воспроизводит бинарный файл как анимацию.
Вычисляет задержку между кадрами на основе FPS.
"""
# Вычисляем количество байт в одной строке (ширина / 8)
bytes_per_row = width // 8
# Вычисляем общий размер одного кадра в байтах
frame_bytes = bytes_per_row * height
# Создаем буфер для чтения сырых данных
raw_buf = bytearray(frame_bytes)
# Настройки для чтения чанками (блоками строк)
chunk_rows = 20
chunk_raw_bytes = bytes_per_row * chunk_rows
chunk_rgb = bytearray(width * chunk_rows * 2)
# Разделяем цвета на старшие и младшие байты для быстрой записи
fg_h = (fg_color >> 8) & 0xFF
fg_l = fg_color & 0xFF
bg_h = (bg_color >> 8) & 0xFF
bg_l = bg_color & 0xFF
# Вычисляем отступы для центрирования анимации
x_offset = (320 - width) // 2
y_offset = (240 - height) // 2 - 10
# Вычисляем задержку в миллисекундах на один кадр
delay_ms = int(1000 / fps) if fps > 0 else 0
try:
with open(filename, 'rb') as f:
while True:
# Читаем один кадр
read_bytes = f.readinto(raw_buf)
if read_bytes < frame_bytes:
break # Конец файла
# Засекаем время начала отрисовки кадра
t_start = time.ticks_ms()
# Устанавливаем окно для текущего кадра
display.set_window(x_offset, y_offset, x_offset + width - 1, y_offset + height - 1)
# Отрисовываем кадр чанками (блоками строк) для экономии памяти
for offset in range(0, frame_bytes, chunk_raw_bytes):
chunk_mem = memoryview(raw_buf)[offset : offset + chunk_raw_bytes]
decode_chunk_normal(chunk_mem, chunk_rgb, chunk_raw_bytes, fg_h, fg_l, bg_h, bg_l)
display.write_data(chunk_rgb)
# Выдерживаем задержку для соблюдения FPS
if delay_ms > 0:
elapsed = time.ticks_diff(time.ticks_ms(), t_start)
sleep_time = delay_ms - elapsed
if sleep_time > 0:
time.sleep_ms(sleep_time)
except OSError:
print(f"Ошибка: Файл {filename} не найден!")
# ==============================================================================
# БЛОК 5: КЛАСС УПРАВЛЕНИЯ ДАТЧИКАМИ (SensorManager)
# Отвечает за опрос DHT11 и ультразвукового датчика с фильтрацией.
# ==============================================================================
class SensorManager:
"""
Менеджер датчиков. Инкапсулирует логику работы с DHT11 и HC-SR04.
"""
def __init__(self, trig_pin, echo_pin, dht_pin):
# Инициализация ультразвукового датчика
self.trig = Pin(trig_pin, Pin.OUT)
self.echo = Pin(echo_pin, Pin.IN)
# Инициализация датчика температуры и влажности
self.dht_sensor = dht.DHT11(Pin(dht_pin))
# Переменные для хранения последних показаний
self.last_distance = 999
self.last_temperature = 0
self.last_humidity = 0
def _single_measure(self):
"""
Одиночный замер расстояния ультразвуком.
Возвращает расстояние в сантиметрах или 999 в случае ошибки.
"""
self.trig.value(0)
time.sleep_us(2)
self.trig.value(1)
time.sleep_us(10)
self.trig.value(0)
try:
# Ждем эхо-сигнал (максимум 30 мс)
duration = time_pulse_us(self.echo, 1, 30000)
if duration > 0:
# Скорость звука 343 м/с = 0.0343 см/мкс. Делим на 2 (путь туда-обратно)
return (duration * 0.0343) / 2
return 999
except OSError:
return 999
def measure_distance_filtered(self):
"""
Фильтрованный замер. Делает 3 замера и отсеивает случайные выбросы.
Это защищает от ложных срабатываний датчика.
"""
d1 = self._single_measure()
time.sleep_ms(5)
d2 = self._single_measure()
time.sleep_ms(5)
d3 = self._single_measure()
# Если два значения близки, возвращаем их среднее
if abs(d1 - d2) < 15: return (d1 + d2) / 2
if abs(d2 - d3) < 15: return (d2 + d3) / 2
if abs(d1 - d3) < 15: return (d1 + d3) / 2
# Если все разные (шум), возвращаем медиану
return sorted([d1, d2, d3])[1]
def check_edge(self):
"""
Проверяет наличие края стола.
Возвращает True, если обнаружен резкий перепад (обрыв).
"""
dist = self.measure_distance_filtered()
# Если было меньше 10 см, а стало больше 30 см - это обрыв
if self.last_distance < 10 and dist > 30:
return True
self.last_distance = dist
return False
def read_dht(self):
"""
Читает температуру и влажность.
Возвращает кортеж (температура, влажность) или (None, None) при ошибке.
"""
try:
self.dht_sensor.measure()
self.last_temperature = self.dht_sensor.temperature()
self.last_humidity = self.dht_sensor.humidity()
return self.last_temperature, self.last_humidity
except OSError:
return None, None
# ==============================================================================
# БЛОК 6: КЛАСС УПРАВЛЕНИЯ МОТОРАМИ (MotorDriver)
# Инкапсулирует логику подачи сигналов на драйвер L9110S.
# ==============================================================================
class MotorDriver:
"""
Драйвер моторов. Управляет двумя каналами (A и B).
"""
def __init__(self, a1a_pin, a1b_pin, b1a_pin, b1b_pin):
# Инициализация пинов как выходов
self.M_A_1A = Pin(a1a_pin, Pin.OUT)
self.M_A_1B = Pin(a1b_pin, Pin.OUT)
self.M_B_1A = Pin(b1a_pin, Pin.OUT)
self.M_B_1B = Pin(b1b_pin, Pin.OUT)
def stop(self):
"""Полная остановка всех моторов."""
self.M_A_1A.value(0)
self.M_A_1B.value(0)
self.M_B_1A.value(0)
self.M_B_1B.value(0)
def forward(self):
"""Движение вперед."""
self.M_A_1A.value(1)
self.M_A_1B.value(0)
self.M_B_1A.value(1)
self.M_B_1B.value(0)
def backward(self):
"""Движение назад."""
self.M_A_1A.value(0)
self.M_A_1B.value(1)
self.M_B_1A.value(0)
self.M_B_1B.value(1)
def turn_left(self):
"""Поворот налево (разворот на месте)."""
self.M_A_1A.value(0)
self.M_A_1B.value(1)
self.M_B_1A.value(1)
self.M_B_1B.value(0)
def turn_right(self):
"""Поворот направо (разворот на месте)."""
self.M_A_1A.value(1)
self.M_A_1B.value(0)
self.M_B_1A.value(0)
self.M_B_1B.value(1)
# ==============================================================================
# БЛОК 7: КЛАСС УПРАВЛЕНИЯ СЕРВОПРИВОДОМ (ServoManager)
# Отвечает за поворот шеи робота.
# ==============================================================================
class ServoManager:
"""
Управление сервоприводом (SG90).
"""
def __init__(self, pin):
self.servo = PWM(Pin(pin), freq=50)
def set_angle(self, angle):
"""
Устанавливает угол поворота (0-180 градусов).
Переводит угол в скважность (duty) для PWM.
"""
# 4000 - это 0 градусов, 8000 - это 180 градусов (для 16-битного PWM)
duty = int(4000 + (angle / 180) * 4000)
self.servo.duty_u16(duty)
# ==============================================================================
# БЛОК 8: КЛАСС УПРАВЛЕНИЯ РОБОТОМ (RobotController)
# Главный класс, связывающий всё воедино. Использует машину состояний.
# ==============================================================================
class RobotController:
"""
Ядро робота. Управляет состояниями, таймерами и взаимодействием модулей.
"""
# Определяем константы состояний
STATE_IDLE = 0 # Стоим, ждем
STATE_MOVING = 1 # Двигаемся
STATE_LOOKING = 2 # Осматриваемся
STATE_EMERGENCY = 3 # Экстренное торможение
def __init__(self):
# Инициализация аппаратных модулей
self.spi = SPI(2, baudrate=40000000, sck=Pin(18), mosi=Pin(23), miso=Pin(19))
self.display = DisplayDriver(self.spi, cs_pin=5, dc_pin=2, rst_pin=4)
self.display.clear(0x0000)
# Инициализация текстового дисплея
self.text_display = None
if TEXT_LIB_AVAILABLE:
try:
self.text_display = Display(self.spi, dc=Pin(2), cs=Pin(5), rst=Pin(4), width=320, height=240, rotation=90)
except:
pass
# Инициализация менеджеров
self.sensors = SensorManager(trig_pin=16, echo_pin=35, dht_pin=33)
self.motors = MotorDriver(a1a_pin=26, a1b_pin=27, b1a_pin=13, b1b_pin=14)
self.servo = ServoManager(pin=25)
# Переменные машины состояний
self.current_state = self.STATE_IDLE
self.state_timer = time.ticks_ms() # Таймер текущего состояния
self.action_timer = 0 # Таймер длительности движения
self.last_dht_time = time.ticks_ms()
# Случайные параметры
self.wait_time = random.randint(10, 17) * 1000 # Время ожидания (10-17 сек)
# Флаги
self.looked_around = False
self.has_blinked = False
self.action_type = ''
def update_display_text(self, t, h):
"""Обновляет текст в нижней полосе экрана."""
if self.text_display:
# Очищаем нижние 20 пикселей
self.text_display.fill_rectangle(0, 220, 320, 20, color565(0, 0, 0))
# Пишем текст
self.text_display.draw_text8x8(10, 225, f"T: {t}C H: {h}%", color565(255, 255, 255), color565(0,0,0), 0)
def show_text_on_screen(self, text):
"""Выводит произвольный текст на экран."""
if self.text_display:
self.text_display.fill_rectangle(0, 20, 320, 240, color565(0, 0, 0))
self.text_display.draw_text8x8(10, 100, text, color565(255, 255, 255), color565(0,0,0), 0)
def turn_around_180(self):
"""Разворот на 180 градусов."""
self.motors.turn_left()
time.sleep(0.7)
self.motors.stop()
def execute_emergency(self):
"""Экстренная остановка при обнаружении края."""
print("!!! КРАЙ СТОЛА !!!")
self.motors.stop()
self.turn_around_180()
self.motors.forward()
time.sleep(1.5) # Отъезжаем назад
self.motors.stop()
# Сбрасываем таймеры и генерируем новое время ожидания
self.current_state = self.STATE_IDLE
self.state_timer = time.ticks_ms()
self.wait_time = random.randint(10, 17) * 1000
self.looked_around = False
self.has_blinked = False
def start_new_cycle(self):
"""Начинает новый цикл ожидания."""
self.wait_time = random.randint(10, 17) * 1000
self.state_timer = time.ticks_ms()
self.current_state = self.STATE_IDLE
self.looked_around = False
self.has_blinked = False
self.action_type = ''
def run(self):
"""Главный цикл программы."""
print("Робот LOIK запущен! (v3.0)")
while True:
now = time.ticks_ms()
# --- 1. ОБНОВЛЕНИЕ ДАТЧИКА DHT11 ---
if time.ticks_diff(now, self.last_dht_time) > 3000:
t, h = self.sensors.read_dht()
if t is not None:
self.update_display_text(t, h)
self.last_dht_time = now
# --- 2. ГЛОБАЛЬНАЯ ПРОВЕРКА КРАЯ (ВЫСШИЙ ПРИОРИТЕТ) ---
if self.current_state != self.STATE_EMERGENCY:
if self.sensors.check_edge():
self.execute_emergency()
continue # Пропускаем остальную логику, начинаем заново
# --- 3. МАШИНА СОСТОЯНИЙ ---
# --- СОСТОЯНИЕ: ОЖИДАНИЕ ---
if self.current_state == self.STATE_IDLE:
elapsed = time.ticks_diff(now, self.state_timer)
# 3.1. Моргание раз в 10 секунд
if elapsed >= 10000 and not self.has_blinked:
print("[IDLE] Моргаем...")
play_animation(self.display, 'BLINK.bin', fps=30)
self.has_blinked = True
# 3.2. Осмотр ровно на 10-й секунде
if elapsed >= 10000 and not self.looked_around:
wait_seconds = self.wait_time // 1000
if wait_seconds > 14:
print(f"[IDLE] Осматриваемся (ждем {wait_seconds} сек)...")
self.servo.set_angle(180); time.sleep(2)
self.servo.set_angle(0); time.sleep(4)
self.servo.set_angle(90); time.sleep(1)
self.looked_around = True
# 3.3. Начало движения (если время вышло)
if elapsed >= self.wait_time:
action = random.choice(['forward', 'forward', 'forward', 'left', 'right'])
duration = random.uniform(1.0, 2.5)
self.action_type = action
self.action_timer = time.ticks_add(now, int(duration * 1000))
self.current_state = self.STATE_MOVING
print(f"[MOVE] Едем {action} ({duration:.1f} сек)")
# --- СОСТОЯНИЕ: ДВИЖЕНИЕ ---
elif self.current_state == self.STATE_MOVING:
# Проверяем, не пора ли остановиться
if time.ticks_diff(now, self.action_timer) >= 0:
self.motors.stop()
print("[MOVE] Остановка. Уходим в ожидание.")
self.start_new_cycle()
else:
# Выполняем движение с проверкой края (микро-шагами)
if self.action_type == 'forward':
self.motors.forward()
time.sleep_ms(50)
self.motors.stop()
elif self.action_type == 'left':
self.motors.turn_left()
time.sleep_ms(50)
self.motors.stop()
elif self.action_type == 'right':
self.motors.turn_right()
time.sleep_ms(50)
self.motors.stop()
# --- ФОНОВАЯ ЗАДЕРЖКА ---
time.sleep_ms(10)
# ==============================================================================
# БЛОК 9: ОБРАБОТКА КОМАНД BLUETOOTH
# ==============================================================================
# Инициализация робота
robot = RobotController()
# Инициализация Bluetooth
ble = bluetooth.BLE()
sp = BLESimplePeripheral(ble, name="LOIK")
def on_rx(data):
"""
Обработчик входящих команд по Bluetooth.
"""
try:
cmd = data.decode('utf-8').strip().lower()
print(f"BT Команда: {cmd}")
# --- КОМАНДЫ ДВИЖЕНИЯ ---
if cmd == 'y':
# Проверка края перед принудительным движением
if robot.sensors.check_edge():
robot.execute_emergency()
else:
robot.current_state = robot.STATE_MOVING
robot.action_type = 'forward'
robot.action_timer = time.ticks_add(time.ticks_ms(), 1000)
elif cmd == 'o':
robot.motors.backward(); time.sleep(1); robot.motors.stop()
elif cmd == 'r':
robot.motors.turn_left(); time.sleep(0.5); robot.motors.stop()
elif cmd == 't':
robot.motors.turn_right(); time.sleep(0.5); robot.motors.stop()
# --- КОМАНДЫ ШЕИ ---
elif cmd == 'gleft':
robot.servo.set_angle(180); time.sleep(1.5); robot.servo.set_angle(90)
elif cmd == 'gright':
robot.servo.set_angle(0); time.sleep(1.5); robot.servo.set_angle(90)
# --- АНИМАЦИИ ---
elif cmd == 'w':
play_animation(robot.display, 'LOVE.bin', fps=30)
elif cmd == 'q':
play_animation(robot.display, 'hello.bin', fps=30)
# --- ТЕКСТ ---
elif cmd.startswith('text:'):
text_to_show = cmd.split(':')[1]
robot.show_text_on_screen(text_to_show)
# --- МОРГАНИЕ ---
elif cmd.startswith('blink:'):
try:
count = int(cmd.split(':')[1])
for _ in range(count):
play_animation(robot.display, 'blink.bin', fps=30)
time.sleep(0.5)
except ValueError:
print("Ошибка: неверный формат команды моргания")
elif cmd == 'stop':
robot.motors.stop()
robot.start_new_cycle()
except Exception as e:
print("Ошибка обработки команды:", e)
# Привязываем обработчик к Bluetooth
sp.on_write(on_rx)
# ==============================================================================
# БЛОК 10: ЗАПУСК ПРОГРАММЫ
# ==============================================================================
if __name__ == "__main__":
# Запускаем главный цикл робота
robot.run()