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.
99 lines
3.1 KiB
99 lines
3.1 KiB
1 month ago
|
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()
|