You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
98 lines
3.1 KiB
98 lines
3.1 KiB
import RPi.GPIO as GPIO |
|
import serial |
|
import time |
|
import numpy as np |
|
import matplotlib.pyplot as plt |
|
from matplotlib.animation import FuncAnimation |
|
import keyboard |
|
|
|
# Настройки GPIO |
|
IN1 = 23 |
|
IN2 = 24 |
|
IN3 = 17 |
|
IN4 = 27 |
|
ENA = 26 |
|
ENB = 19 |
|
|
|
GPIO.setwarnings(False) |
|
GPIO.setmode(GPIO.BCM) |
|
GPIO.setup(IN1, GPIO.OUT) |
|
GPIO.setup(IN2, GPIO.OUT) |
|
GPIO.setup(IN3, GPIO.OUT) |
|
GPIO.setup(IN4, GPIO.OUT) |
|
GPIO.setup(ENA, GPIO.OUT) |
|
GPIO.setup(ENB, GPIO.OUT) |
|
|
|
# Параметры для лидара |
|
lidar_port = '/dev/ttyUSB1' # Убедитесь, что это правильный порт |
|
baud_rate = 115200 |
|
|
|
# Настройка серийного соединения |
|
ser = serial.Serial(lidar_port, baud_rate, timeout=1) |
|
|
|
# Параметры скорости |
|
speed = 10 # Регулируйте скорость по необходимости |
|
GPIO.output(ENA, speed) |
|
GPIO.output(ENB, speed) |
|
|
|
# Функции управления движением |
|
def move_forward(): |
|
GPIO.output(IN1, GPIO.HIGH) |
|
GPIO.output(IN2, GPIO.LOW) |
|
GPIO.output(IN3, GPIO.HIGH) |
|
GPIO.output(IN4, GPIO.LOW) |
|
print("Робот движется вперед") |
|
|
|
def move_backward(): |
|
GPIO.output(IN1, GPIO.LOW) |
|
GPIO.output(IN2, GPIO.HIGH) |
|
GPIO.output(IN3, GPIO.LOW) |
|
GPIO.output(IN4, GPIO.HIGH) |
|
print("Робот движется назад") |
|
|
|
def stop(): |
|
GPIO.output(IN1, GPIO.LOW) |
|
GPIO.output(IN2, GPIO.LOW) |
|
GPIO.output(IN3, GPIO.LOW) |
|
GPIO.output(IN4, GPIO.LOW) |
|
print("Робот остановлен") |
|
|
|
def read_lidar_data(): |
|
try: |
|
line = ser.readline().decode('utf-8', errors='ignore').strip() |
|
if line: |
|
print(f"Считанные данные от лидара: {line}") # Отладка |
|
return line # Вернем считанные данные |
|
else: |
|
print("Нет данных") |
|
return None |
|
except Exception as e: |
|
print(f"Ошибка чтения данных от лидара: {e}") |
|
return None |
|
|
|
# Визуализация карты препятствий |
|
def update_plot(frame): |
|
data = read_lidar_data() # Получаем данные от лидара |
|
if data is not None: |
|
# Логика для визуализации данных |
|
# Вставьте здесь ваш код для построения карты препятствий |
|
pass # Замените на свою логику |
|
|
|
# Основной цикл |
|
def main(): |
|
print("Нажмите 'W' для запуска автономного движения и ' ' (пробел) для остановки.") |
|
while True: |
|
if keyboard.is_pressed('w'): |
|
move_forward() |
|
print("Автономное движение включено") |
|
if keyboard.is_pressed('s'): |
|
move_backward() |
|
if keyboard.is_pressed(' '): |
|
stop() |
|
print("Автономное движение остановлено") |
|
# Добавьте паузу, чтобы избежать высокой загрузки CPU |
|
time.sleep(0.1) |
|
|
|
# Запуск программы |
|
if __name__ == "__main__": |
|
main()
|
|
|