import numpy as np
import matplotlib.pyplot as plt

pole = 3
#测试pid归一化处理
rpm_2_omega = 2 * np.pi * pole / 60 # omega = 2*PI*rpm*pole/60

rpm_set = 1256
rpm_max = 2123
rpm_min = 0

omega_set = int(rpm_2_omega * rpm_set)
omega_max = int(rpm_2_omega * rpm_max)
omega_min = int(rpm_2_omega * rpm_min)
omega_ramp = 0
kp = 0.3
ki = 0.01


omega_set_norm = int((omega_set/(omega_max - omega_min)) * 32767)
omega_ramp_norm = 0
kp_norm = 0.9 * 32767
ki_norm = 0.6 * 32767



show_omega = []
show_out = []

def pid_raw_data():
    global omega_ramp
    sum = 0
    err = 0
    for i in range(400):
        err = omega_set - omega_ramp
        out = kp * err + ki * sum
        sum = sum + err
        omega_ramp += out/6
        show_omega.append(omega_ramp)
        show_out.append(out)


show_omega_norm = []
show_out_norm = []
def pid_norm_data():
    global omega_ramp_norm
    sum = 0
    err = 0
    for i in range(400):
        err = omega_set_norm - omega_ramp_norm
        out = kp_norm * err + ki_norm * sum
        sum = sum + err
        omega_ramp_norm += out/(6*32767)
        show_omega_norm.append(omega_ramp_norm / 32767 * (omega_max - omega_min))
        show_out_norm.append(out / 32767)


if __name__ == "__main__":


    print(rpm_set, omega_set, omega_set_norm )

    pid_raw_data()
    pid_norm_data()

    plt.figure("pid")
    # plt.plot(show_omega, label='omega')
    plt.plot(show_out, label='out')

    plt.figure("pid norm")
    # plt.plot(show_omega_norm, label='omega')
    plt.plot(show_out_norm, label='out')
    plt.show()