import numpy as np
import matplotlib.pyplot as plt

# Parâmetros do motor BLDC (valores fictícios)
R = 2.875  # Resistência (MEDIDO)
L = 0.0048572  # Indutância
Kt = 0.31  # Constante eletromotriz
J = 0.8  # Inércia do motor
B = 4.0  # Coeficiente de atrito

# Implementação do Controlador PID
def pid_controller(Kp, Ki, Kd, setpoint, current_value, prev_error, integral):
    error = setpoint - current_value
    integral += error
    derivative = error - prev_error
    output = Kp * error + Ki * integral + Kd * derivative
    prev_error = error
    return output, prev_error, integral

# Parâmetros do controlador PID (ajuste conforme necessário)
Kp = 1.85
Ki = 2.1
Kd = 0.08

# Condições iniciais
setpoint = 50.0  # Referência desejada
rpm_atual = 0.0  # Valor atual do motor
integral = 0.0
prev_error = 0.0

# Lista para armazenar os valores da saída do motor
rpm_aferido = []

# Simulação ao longo do tempo
tempo = np.arange(0, 3, 0.01)
for t in tempo:
    control_output, prev_error, integral = pid_controller(Kp, Ki, Kd, setpoint, rpm_atual, prev_error, integral)
    # Atualize o modelo do motor com a saída do controlador
    pwm = control_output  # Esta é a tensão aplicada ao motor (modelo simplificado)
    rpm_atual = rpm_atual + (pwm - Kt * rpm_atual) / (L * R) * 0.01  # Modelo do motor BLDC simplificado
    rpm_aferido.append(rpm_atual)

# Visualização dos Resultados
plt.figure()
plt.plot(tempo, rpm_aferido)
plt.xlabel('Tempo (s)')
plt.ylabel('RPM')
plt.title('Resposta do Motor BLDC com PID')
plt.grid(True)
plt.show()