import numpy as np
import matplotlib.pyplot as plt

import k_motor_mode

#主要测试积分饱和的超调对比
#对比十分明显 积分过调回长时间不回调
out = []
sum = []
err = []
outMax = 400
outMin = -outMax
speed = []

#kc参数抗积分饱和,ki kc一般设为一样的数值
def PI_excess(kp, ki, kc, ref, velue):
    err.append(ref - velue)
    excess = sum[-1] + kp * err[-1]

    if excess>outMax:
        out.append(outMax)
    elif excess<outMin:
        out[-1] = outMin
        out.append(outMin)
    else:
        out.append(excess)

    excess = excess - out[-1]
    sum.append(sum[-1] + ki * err[-1] - kc * excess)

    return out[-1]

if __name__ == "__main__":
    m0 = k_motor_mode.k_motor()
    m1 = k_motor_mode.k_motor()

    outMax = m0.motorPwmMax - 100
    outMin = m0.motorPwmMin
    time = np.arange(150)#模拟一个时间线,1ms采样周期,运行一次pid

    #1-----------
    target = 0
    out.append(0)
    sum.append(0)
    speed.append(0)
    
    for i in range(len(time)):
        if i == 2:
            target = 300
        elif i == 50:
            target = 900
        elif i == 100:
            target = 100
        PI_excess(1.3, 0.9, 0, target, speed[-1])#输出pwm
        tmp = m0.motor_mode(out[-1])
        speed.append(tmp) 

    out1 = out[:-1]
    sum1 = sum[:-1]
    err1 = err
    speed1 = speed[:-1]
    #1-----------

    out.clear()
    sum.clear()
    err.clear()
    speed.clear()

    #2-----------
    target = 0
    out.append(0)
    sum.append(0)
    speed.append(0)
    for i in range(len(time)):
        if i == 2:
            target = 300
        elif i == 50:
            target = 900
        elif i == 100:
            target = 100
        PI_excess(1.3, 0.9, 0.9, target, speed[-1])
        tmp = m1.motor_mode(out[-1])
        speed.append(tmp) 

    out2 = out[:-1]
    sum2 = sum[:-1]
    err2 = err
    speed2 = speed[:-1]
    #2-----------


    plt.figure("PID show")
    plt.plot(time, err1, label='err1')
    plt.plot(time, out1, label='out1')
    plt.plot(time, sum1, label='sum1')
    plt.plot(time, speed1, label='speed1')
    plt.legend()

    plt.plot(time, err2, label='err2')
    plt.plot(time, out2, label='out2')
    plt.plot(time, sum2, label='sum2')
    plt.plot(time, speed2, label='speed2')
    plt.legend()
    
    plt.figure("PID speed")
    plt.plot(time, speed1, label='speed1')
    plt.plot(time, speed2, label='speed2')
    plt.legend()

    plt.figure("PID pwm")
    plt.plot(time, out1, label='out1')
    plt.plot(time, out2, label='out2')
    plt.legend()

    # plt.ylabel('alpha-beta')
    # plt.legend()
    plt.show()