'''
*** PID python main.py
'''

class PID:
    def __init__(self, Kp, Ki, Kd):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.last_error = 0
        self.integral = 0

    def update(self, error, dt):
        derivative = (error - self.last_error) / dt
        self.integral += error * dt
        output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
        self.last_error = error
        return output

######################################################################
# Example program
Kp = 1
Ki = 0.9
Kd = 0.05

pid = PID(Kp=Kp, Ki=Ki, Kd=Kd)

setpoint = 10.0
dt = 0.05
time = 0.0
current_value = 0.0

'''
while (1):
    error = setpoint - current_value
    control_signal = pid.update(error, dt)
    current_value += control_signal * dt
    print(f"time: {time:.2f}, setpoint: {setpoint:.2f}, current value: {current_value:.2f}, error: {error:.2f}, control signal: {control_signal:.2f}")
    time += dt
'''


while time < 15.0:
    error = setpoint - current_value
    control_signal = pid.update(error, dt)
    current_value += control_signal * dt
    print(f"time: {time:.2f}, setpoint: {setpoint:.2f}, current value: {current_value:.2f}, error: {error:.2f}, control signal: {control_signal:.2f}")
    time += dt