from machine import Pin
from rp2 import PIO, StateMachine, asm_pio
import utime

# PID szabályzó paraméterei
kp = 1.0  # Proportionális
ki = 0.1  # Integráló
kd = 0.01  # Derivatív

# PID szabályzó változói
previous_error = 0
integral = 0

# A DC motor és enkóder GPIO pinei
motor_pin = Pin(2)
encoder_pin_A = Pin(3)
encoder_pin_B = Pin(4)

# Enkóder állapotgép programja
@asm_pio(sideset_init=PIO.OUT_LOW, out_init=PIO.OUT_LOW, out_shiftdir=PIO.SHIFT_RIGHT)
def encoder_program():
    pull()
    mov(x, osr)
    jmp(not_x, "skip_high")
    in_(pins, 1)
    jmp("done")
    label("skip_high")
    nop()
    label("done")

# Inicializáljuk a PIO és az enkóder állapotgépet
pio_encoder = PIO(0)
sm_encoder = StateMachine(1, encoder_program, freq=1000000, in_base=encoder_pin_A)
sm_encoder.active(1)

# PID szabályzó függvény
def pid_control(target_position, current_position):
    global previous_error, integral

    error = target_position - current_position
    integral += error
    derivative = error - previous_error

    output = kp * error + ki * integral + kd * derivative

    previous_error = error

    return output

# DC motor vezérlése és PID szabályzás
def control_motor(target_position):
    global integral

    while True:
        # Olvassuk be az enkóder pozícióját
        encoder_value = sm_encoder.get()

        # Számítsuk ki a PID kimenetét
        output = pid_control(target_position, encoder_value)

        # Motor vezérlése a PID kimenettel
        motor_pin.value(output > 0)
        motor_pin.duty_u16(abs(int(output)))

        # Kis várakozás
        utime.sleep_ms(10)

# Teszt: Mozgassuk a motort a 100 pozíció felé
target_position = 100
control_motor(target_position)