from machine import Pin, PWM, time_pulse_us
import time

# ================= PIN CONFIG =================
TRIG = Pin(2, Pin.OUT)
ECHO = Pin(3, Pin.IN)

servo = PWM(Pin(17))
servo.freq(50)

relay = Pin(16, Pin.OUT)

# ================= SETPOINT ===================
setpoint = 30.0   # cm

# ================= PID PARAMETER ==============
Kp = 0.8
Ki = 0.02
Kd = 0.1

integral = 0
prev_error = 0
dt = 0.1  # 100 ms

# GANTI MODE: "P" | "PI" | "PID"
mode = "P"

# ================= FUNCTION ===================
def read_distance():
    TRIG.low()
    time.sleep_us(2)
    TRIG.high()
    time.sleep_us(10)
    TRIG.low()

    duration = time_pulse_us(ECHO, 1, 30000)
    if duration < 0:
        return None

    distance = (duration * 0.0343) / 2

    # BATASI JARAK MAKSIMUM
    if distance > 100:
        distance = 100

    return distance

def set_servo(angle):
    angle = max(0, min(180, angle))
    duty = int(1638 + (angle / 180) * (8192 - 1638))
    servo.duty_u16(duty)

# ================= MAIN LOOP ==================
while True:
    distance = read_distance()

    if distance is None:
        continue

    error = setpoint - distance
    integral += error * dt
    derivative = (error - prev_error) / dt

    # ===== PILIH MODE KONTROL =====
    if mode == "P":
        output = Kp * error

    elif mode == "PI":
        output = (Kp * error) + (Ki * integral)

    elif mode == "PID":
        output = (Kp * error) + (Ki * integral) + (Kd * derivative)

    # BATASI OUTPUT PID
    output = max(-45, min(45, output))

    servo_angle = 90 + output
    set_servo(servo_angle)

    # ===== KONTROL RELAY =====
    if distance < 15:
        relay.on()
    else:
        relay.off()

    # ===== MONITOR SERIAL =====
    print(
        "Mode:", mode,
        "| Jarak:", round(distance, 2), "cm",
        "| Error:", round(error, 2),
        "| Servo:", round(servo_angle, 2)
    )

    prev_error = error
    time.sleep(dt)