from machine import Pin
from hcsr04 import HCSR04
import time
import _thread as thread

# Configuração dos pinos do sensor ultrassônico
trigger_pin = Pin(17, Pin.OUT)
echo_pin = Pin(16, Pin.IN)

# Inicializa o objeto HCSR04
sensor = HCSR04(trigger_pin, echo_pin)

# Definição dos parâmetros do controle PID
target_position = 200  # Setpoint em mm
Kp = 0.5  # Ganho proporcional
Ki = 0.1  # Ganho integral
Kd = 0.0  # Ganho derivativo
dt = 0.02  # Intervalo de tempo em segundos

# Variáveis para o controle PID
integral = 0
previous_error = 0

# Função de filtro exponencial
coefficient = 1  # Coeficiente do filtro exponencial
result = 0  # Inicializa o resultado do filtro exponencial

def set_filter_coefficient(new_coefficient):
    global coefficient
    if 0 <= new_coefficient <= 1:
        coefficient = new_coefficient

def filter_sample(sample):
    global coefficient, result
    result = coefficient * sample + (1 - coefficient) * result
    return result

# Função de medição da distância ultrassônica
def measure_distance():
    global distance
    while True:
        distance = sensor.distance_cm()
        time.sleep(0.5)

# Inicia a thread para medição da distância
thread.start_new_thread(measure_distance, ())

# Loop principal do controle PID
while True:
    # Aplica o filtro exponencial na distância medida
    filtered_distance = filter_sample(distance)

    # Calcula o erro
    error = target_position - filtered_distance

    # Calcula os componentes do controle PID
    proportional = Kp * error
    integral += Ki * error * dt
    derivative = Kd * (error - previous_error) / dt

    # Calcula a saída do controle PID
    output = proportional + integral + derivative

    # Atualiza o erro anterior
    previous_error = error

    # Imprime a posição atual e a saída do controle PID
    print("Posição: {:.2f} mm, Saída: {:.2f}".format(filtered_distance, output))

    # Aguarda o intervalo de tempo especificado
    time.sleep(dt)