---

import pygame
import RPi.GPIO as GPIO
import sys
import time

# Настройка GPIO
GPIO.setmode(GPIO.BCM)

# Пины моторов: M1-M8
motor_pins = [26, 19, 13, 16, 27, 20, 23, 21]

# Инициализация пинов как выходов
for pin in motor_pins:
    GPIO.setup(pin, GPIO.OUT)
    GPIO.output(pin, GPIO.LOW)
for pin in motor_pins:
    GPIO.output(pin, GPIO.LOW)

# Отдельные переменные для удобства
M1, M2, M3, M4 = 26, 19, 13, 16  # Передние моторы
M5, M6, M7, M8 = 27, 20, 23, 21  # Задние моторы

# Группы колёс
front_left = [M1, M2]   # Передние левые
front_right = [M3, M4]  # Передние правые
rear_left = [M5, M6]    # Задние левые
rear_right = [M7, M8]   # Задние правые

# Инициализация Pygame
pygame.init()

# Проверка наличия джойстика
if pygame.joystick.get_count() == 0:
    pygame.quit()
    sys.exit()

# Инициализация джойстика
joystick = pygame.joystick.Joystick(0)
joystick.init()

# Функции управления моторами
def stop_all_motors():
    """Остановить все моторы"""
    for pin in motor_pins:
        GPIO.output(pin, GPIO.LOW)

def stop_wheel_group(wheel_group):
    """Остановить группу колёс"""
    for pin in wheel_group:
        GPIO.output(pin, GPIO.LOW)

def move_forward():
    """Движение вперёд: все чётные пины LOW, нечётные HIGH"""
    GPIO.output(M2, 1)
    GPIO.output(M1, 0)
    GPIO.output(M3, 0)
    GPIO.output(M4, 1)
    GPIO.output(M5, 0)
    GPIO.output(M6, 1)
    GPIO.output(M7, 0)
    GPIO.output(M8, 1)

def move_backward():
    """Движение назад: все чётные пины HIGH, нечётные LOW"""
    GPIO.output(M2, 0)
    GPIO.output(M1, 1)
    GPIO.output(M3, 1)
    GPIO.output(M4, 0)
    GPIO.output(M5, 1)
    GPIO.output(M6, 0)
    GPIO.output(M7, 1)
    GPIO.output(M8, 0)

def move_left():
    GPIO.output(M2, 0)
    GPIO.output(M1, 1)
    GPIO.output(M3, 0)
    GPIO.output(M4, 1)
    GPIO.output(M5, 1)
    GPIO.output(M6, 0)
    GPIO.output(M7, 0)
    GPIO.output(M8, 1)
def move_right():
    GPIO.output(M2, 1)
    GPIO.output(M1, 0)
    GPIO.output(M3, 1)
    GPIO.output(M4, 0)
    GPIO.output(M5, 0)
    GPIO.output(M6, 1)
    GPIO.output(M7, 1)
    GPIO.output(M8, 0)
stop_all_motors()
# Основной цикл
try:
    while True:
        # Обработка событий Pygame
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                raise KeyboardInterrupt

        # Получение состояния осей джойстика
        axis_y = joystick.get_axis(1)  # Левый стик Y (вперёд-назад)
        axis_x = joystick.get_axis(0)  # Левый стик X (влево-вправо)

        # Настройка углов (порогов) для управления
        threshold_xy = 0.1  # Общий порог чувствительности для фильтрации дрожания
        turn_threshold_left = -0.4  # Порог для поворота влево (более строгий)
        turn_threshold_right = 0.4   # Порог для поворота вправо (более строгий)
        move_threshold_forward = -0.3  # Порог движения вперёд
        move_threshold_backward = 0.3   # Порог движения назад

        

        # Основная логика управления
        if axis_y < move_threshold_forward:  # Движение вперёд
            move_forward()
        elif axis_y > move_threshold_backward:  # Движение назад
            move_backward()
        elif axis_x < turn_threshold_left:
            move_right() 
        elif axis_x > turn_threshold_right:  # Поворот вправо
            move_left()
        else:  # Нейтральное положение — остановка
            stop_all_motors()

          

except KeyboardInterrupt:
    pass # Игнорируем прерывание клавиатуры без вывода
finally:
    stop_all_motors()
    GPIO.cleanup()
    pygame.quit()
    sys.exit()