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.0
Ki = 0.1
Kd = 0.01

# Condições iniciais
setpoint = 10.0  # Referência desejada
current_value = 0.0  # Velocidade angular atual do motor (em rad/s)
integral = 0.0
prev_error = 0.0
prev_value = current_value

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

# Simulação ao longo do tempo
time = np.arange(0, 10, 0.01)
for t in time:
    control_output, prev_error, integral = pid_controller(Kp, Ki, Kd, setpoint, current_value, prev_error, integral)

    # Calcular o torque do motor com inércia e atrito
    torque_motor = J * (current_value - prev_value) / 0.01 + B * current_value + Kt * control_output

    # Atualizar a velocidade angular do motor (em rad/s)
    current_value = current_value + (torque_motor / J) * 0.01  # A inércia J é usada aqui
    prev_value = current_value

    motor_output.append(current_value)

# Visualização dos Resultados
plt.figure()
plt.plot(time, motor_output)
plt.xlabel('Tempo (s)')
plt.ylabel('Velocidade Angular (rad/s)')
plt.title('Resposta do Motor BLDC com PID e Inércia/Atrito')
plt.grid(True)
plt.show()