import RPi.GPIO as GPIO
import time

#Ustawienie trybu numeracji pinów GPIO
GPIO.setmode(GPIO.BCM)

# Definicja pinów sterownika L298N
enA = 2  # Pin PWM dla silnika A
in1 = 3  # Pin kierunku 1 dla silnika A
in2 = 4  # Pin kierunku 2 dla silnika A

enB = 17  # Pin PWM dla silnika B
in3 = 27  # Pin kierunku 1 dla silnika B
in4 = 22  # Pin kierunku 2 dla silnika B

# Konfiguracja pinów jako wyjścia
GPIO.setup(enA, GPIO.OUT)
GPIO.setup(in1, GPIO.OUT)
GPIO.setup(in2, GPIO.OUT)
GPIO.setup(enB, GPIO.OUT)
GPIO.setup(in3, GPIO.OUT)
GPIO.setup(in4, GPIO.OUT)

# Konfiguracja PWM (częstotliwość 100 Hz)
pwmA = GPIO.PWM(enA, 100)
pwmB = GPIO.PWM(enB, 100)

# Uruchomienie PWM z początkową wartością 0%
pwmA.start(0)
pwmB.start(0)

def forward(speed):
    """Funkcja do jazdy do przodu."""
    GPIO.output(in1, GPIO.HIGH)
    GPIO.output(in2, GPIO.LOW)
    GPIO.output(in3, GPIO.HIGH)
    GPIO.output(in4, GPIO.LOW)
    pwmA.ChangeDutyCycle(speed)
    pwmB.ChangeDutyCycle(speed)

def backward(speed):
    """Funkcja do jazdy do tyłu."""
    GPIO.output(in1, GPIO.LOW)
    GPIO.output(in2, GPIO.HIGH)
    GPIO.output(in3, GPIO.LOW)
    GPIO.output(in4, GPIO.HIGH)
    pwmA.ChangeDutyCycle(speed)
    pwmB.ChangeDutyCycle(speed)

def stop():
    """Funkcja do zatrzymania silników."""
    GPIO.output(in1, GPIO.LOW)
    GPIO.output(in2, GPIO.LOW)
    GPIO.output(in3, GPIO.LOW)
    GPIO.output(in4, GPIO.LOW)
    pwmA.ChangeDutyCycle(0)
    pwmB.ChangeDutyCycle(0)

try:
    while True:
        print("Jazda do przodu")
        forward(50)  # Prędkość 50%
        time.sleep(2)

        print("Jazda do tyłu")
        backward(50)  # Prędkość 50%
        time.sleep(2)

        print("Stop")
        stop()
        time.sleep(1)

except KeyboardInterrupt:
    print("Zatrzymanie programu...")
    stop()
    GPIO.cleanup()