from machine import Encoder, Pin, PWM
import time

# 🧭 Encoder 1 – Motor pozíció
motor_encoder = Encoder(0)
motor_encoder.init(phase_a=Pin(25), phase_b=Pin(33), phases=4)

# 🎯 Encoder 2 – Setpoint pozíció
setpoint_encoder = Encoder(1)
setpoint_encoder.init(phase_a=Pin(32), phase_b=Pin(35), phases=4)

# ⚙️ H-bridge vezérlés
in1 = Pin(27, Pin.OUT)
in2 = Pin(26, Pin.OUT)
pwm_out = PWM(Pin(14), freq=1000)
pwm_out.duty(0)

# 📡 Mintavételi időméréshez
sample_pin = Pin(21, Pin.OUT)
sample_pin.value(1)

# 🔁 PID paraméterek
Kp = 10
Kd = 20
error = 0

# ⏱️ Timer megszakítás
interruptCounter = 0
def handleInterrupt(timer):
    global interruptCounter
    interruptCounter += 1

timer0 = machine.Timer(0)
timer0.init(period=1, mode=machine.Timer.PERIODIC, callback=handleInterrupt)

# 🚀 Fő ciklus
print("Servo Drive Start")
while True:
    if interruptCounter > 0:
        interruptCounter -= 1

        # 🔍 Encoder értékek
        motor_pos = motor_encoder.value()
        setpoint_pos = setpoint_encoder.value()

        # 🧠 PID számítás
        error_old = error
        error = setpoint_pos - motor_pos

        if error > 0:
            in1.value(0)
            in2.value(1)
        else:
            in1.value(1)
            in2.value(0)

        P_term = error * Kp
        D_term = (error - error_old) * Kd
        PID = P_term + D_term

        pwm_out.duty(min(abs(PID), 1023))  # PWM duty max 1023

        # ⏱️ Mintavételi jel váltása
        sample_pin.value(not sample_pin.value())