# PID motor speed control simulation for Raspberry Pi Pico (Wokwi)
import time

# --- PID PARAMETERS ---
Kp = 0.8
Ki = 0.2
Kd = 0.05

# --- SIMULATED PLANT (MOTOR) ---
motor_speed = 0.0     # current speed (simulated)
motor_response = 0.1  # how fast motor reacts (0.0–1.0)

setpoint = 100        # target RPM

integral = 0
prev_error = 0

print("Starting PID simulation...\n")

while True:
    error = setpoint - motor_speed
    integral += error * 0.1
    derivative = (error - prev_error) / 0.1

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

    # Simulated motor response
    motor_speed += motor_response * (output - motor_speed)

    prev_error = error

    print(f"Setpoint: {setpoint:4.0f}  | Speed: {motor_speed:6.2f}  | Output: {output:6.2f}")

    time.sleep(0.1)