import RPi.GPIO as GPIO
import time
import keyboard

# Настройка пинов
GPIO.setmode(GPIO.BCM)

# Пины для управления моторами
IN1 = 17
IN2 = 27
IN3 = 22
IN4 = 23

# Пины для управления скоростью (PWM)
ENA = 18
ENB = 19

# Настройка пинов как выходов
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)

# Создание ШИМ (PWM) на пинах ENA и ENB с частотой 1000 Гц
pwm_a = GPIO.PWM(ENA, 1000)
pwm_b = GPIO.PWM(ENB, 1000)

# Старт ШИМ с нулевой скоростью (0% скважности)
pwm_a.start(0)
pwm_b.start(0)

# Функции управления
def move_forward(speed):
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    pwm_a.ChangeDutyCycle(speed)
    pwm_b.ChangeDutyCycle(speed)
    print(f"Moving forward at speed {speed}")

def move_backward(speed):
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.HIGH)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.HIGH)
    pwm_a.ChangeDutyCycle(speed)
    pwm_b.ChangeDutyCycle(speed)
    print(f"Moving backward at speed {speed}")

def move_left(speed):
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.HIGH)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    pwm_a.ChangeDutyCycle(speed)
    pwm_b.ChangeDutyCycle(speed)
    print(f"Turning left at speed {speed}")

def move_right(speed):
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.HIGH)
    pwm_a.ChangeDutyCycle(speed)
    pwm_b.ChangeDutyCycle(speed)
    print(f"Turning right at speed {speed}")

def stop():
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.LOW)
    pwm_a.ChangeDutyCycle(0)
    pwm_b.ChangeDutyCycle(0)
    print("Stopped")

try:
    speed = 50  # Начальная скорость (50% ШИМ)
    print("Use W/A/S/D to move and adjust speed, release key to stop")

    while True:
        # Проверка нажатия клавиш для движения
        if keyboard.is_pressed('w'):
            move_forward(speed)
        elif keyboard.is_pressed('s'):
            move_backward(speed)
        elif keyboard.is_pressed('a'):
            move_left(speed)
        elif keyboard.is_pressed('d'):
            move_right(speed)
        else:
            # Если ни одна из клавиш не нажата, остановить моторы
            stop()

        # Регулировка скорости
        if keyboard.is_pressed('q'):  # Уменьшение скорости
            speed = max(0, speed - 10)
            print(f"Speed decreased to {speed}%")
            time.sleep(0.1)
        elif keyboard.is_pressed('e'):  # Увеличение скорости
            speed = min(100, speed + 10)
            print(f"Speed increased to {speed}%")
            time.sleep(0.1)

        time.sleep(0.05)  # Небольшая задержка для плавного опроса клавиш

except KeyboardInterrupt:
    stop()  # Остановка моторов при прерывании программы
finally:
    pwm_a.stop()
    pwm_b.stop()
    GPIO.cleanup()  # Очистка конфигурации GPIO