import pigpio
import time

PWM_PIN = 18
TACH_PIN = 23

TARGET_TEMP = 40.0

# 初始参数
Kp = 1.0
Ki = 0.0
Kd = 0.0

integral = 0
last_error = 0
last_temp = None

pulse_count = 0
last_time = time.time()

pi = pigpio.pi()

pi.set_PWM_frequency(PWM_PIN, 25000)
pi.set_PWM_range(PWM_PIN, 255)

def tach_cb(gpio, level, tick):
    global pulse_count
    pulse_count += 1

pi.set_mode(TACH_PIN, pigpio.INPUT)
pi.set_pull_up_down(TACH_PIN, pigpio.PUD_UP)
pi.callback(TACH_PIN, pigpio.FALLING_EDGE, tach_cb)

last_tem = 0

def get_temp():
    global last_tem
    with open("/sys/class/thermal/thermal_zone0/temp") as f:
        temp_filtered = 0.8 * last_tem + 0.2 * int(f.read()) / 1000
        last_tem = temp_filtered
        return temp_filtered

def get_rpm():
    global pulse_count, last_time
    now = time.time()
    dt = now - last_time
    count = pulse_count
    pulse_count = 0
    last_time = now
    return (count / 2) / dt * 60

def adaptive_pid(temp):
    global Kp, Ki, Kd, integral, last_error, last_temp

    error = temp - TARGET_TEMP
    integral += error
    integral = max(min(integral, 100), -100)
    derivative = error - last_error

    # ===== 自适应调参 =====

    if last_temp is not None:
        temp_rate = temp - last_temp

        # 响应太慢 → 增大 Kp
        if abs(error) > 5 and abs(temp_rate) < 0.1:
            Kp += 0.001

        # 震荡 → 降低 Kp，提高 Kd
        if abs(temp_rate) > 0.5:
            Kp *= 0.95
            Kd += 0.0005

        # 长期偏差 → 增大 Ki
        if abs(error) > 2:
            Ki += 0.0001

    last_temp = temp
    last_error = error

    output = Kp * error + Ki * integral + Kd * derivative

    return output

try:
    while True:
        temp = get_temp()
        rpm = get_rpm()

        out = adaptive_pid(temp)

        duty = max(10, min(255, int(out * 5)))

        pi.set_PWM_dutycycle(PWM_PIN, duty)

        print(f"\rT={temp:.1f}C RPM={rpm:.0f} PWM={duty} Kp={Kp:.2f} Ki={Ki:.2f} Kd={Kd:.2f}              ", end="")

        time.sleep(0.05)

except KeyboardInterrupt:
    pass

finally:
    pi.set_PWM_dutycycle(PWM_PIN, 0)
    pi.stop()