Q-learning-PID/fupid.py

import numpy as np
import skfuzzy as fuzz
import matplotlib.pyplot as plt
#from  mpl_toolkit.mplot3d import Axes3D
from skfuzzy import control as ctrl
import math


class PIDController:
    def __init__(self, Kp, Ki, Kd):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.last_error = 0
        self.integral = 0

    def control(self, error):
        output = self.Kp * error + self.Ki * self.integral + self.Kd * (error - self.last_error)
        self.integral += error
        self.last_error = error

        return output


class TemperatureSystem:
    def __init__(self, target_temp):
        self.target_temp = target_temp
        self.current_temp = 0

    def update(self, control_singal):
        self.current_temp += control_singal * 1
        return self.current_temp

    def get_error(self):
        return self.target_temp - self.current_temp


def train(controller,system, num_iterations):
    errors = []
    signal = []
    temp = []
    e = ctrl.Antecedent(np.arange(-80, 80, 1), 'e')
    del_e = ctrl.Antecedent(np.arange(-6, 6, 1), 'del_e')
    kp = ctrl.Consequent(np.arange(-0.8, 0.8, 0.01), 'kp')
    ki = ctrl.Consequent(np.arange(-0.1, 0.1, 0.005), 'ki')
    kd = ctrl.Consequent(np.arange(-0.1, 0.1, 0.005), 'kd')

    e['NB'] = fuzz.trimf(e.universe, [-80, -80, -40])
    e['NS'] = fuzz.trimf(e.universe, [-80, -40, 0])
    e['ZO'] = fuzz.trimf(e.universe, [-40, 0, 40])
    e['PS'] = fuzz.trimf(e.universe, [0, 40, 80])
    e['PB'] = fuzz.trimf(e.universe, [40, 80, 80])

    del_e['NB'] = fuzz.trimf(del_e.universe, [-6, -6, -3])
    del_e['NS'] = fuzz.trimf(del_e.universe, [-6, -3, 0])
    del_e['ZO'] = fuzz.trimf(del_e.universe, [-3, 0, 3])
    del_e['PS'] = fuzz.trimf(del_e.universe, [0, 3, 6])
    del_e['PB'] = fuzz.trimf(del_e.universe, [3, 6, 6])

    kp['NB'] = fuzz.trimf(kp.universe, [-0.8, -0.8, -0.4])
    kp['NS'] = fuzz.trimf(kp.universe, [-0.8, -0.4, 0])
    kp['ZO'] = fuzz.trimf(kp.universe, [-0.4, 0, 0.4])
    kp['PS'] = fuzz.trimf(kp.universe, [0, 0.4, 0.8])
    kp['PB'] = fuzz.trimf(kp.universe, [0.4, 0.8, 0.8])

    ki['NB'] = fuzz.trimf(ki.universe, [-0.02, -0.02, -0.01])
    ki['NS'] = fuzz.trimf(ki.universe, [-0.02, -0.01, 0])
    ki['ZO'] = fuzz.trimf(ki.universe, [-0.01, 0, 0.01])
    ki['PS'] = fuzz.trimf(ki.universe, [0, 0.01, 0.02])
    ki['PB'] = fuzz.trimf(ki.universe, [0.01, 0.02, 0.02])

    kd['NB'] = fuzz.trimf(kd.universe, [-0.02, -0.02, -0.01])
    kd['NS'] = fuzz.trimf(kd.universe, [-0.02, -0.01, 0])
    kd['ZO'] = fuzz.trimf(kd.universe, [-0.01, 0, 0.01])
    kd['PS'] = fuzz.trimf(kd.universe, [0, 0.01, 0.02])
    kd['PB'] = fuzz.trimf(kd.universe, [0.01, 0.02, 0.02])

    rule1 = ctrl.Rule((e['NB'] & del_e['NB']) | (e['PS'] & del_e['PB']), kp['PB'])
    rule2 = ctrl.Rule((e['NB'] & del_e['NS']) | (e['NB'] & del_e['ZO']) | (e['NB'] & del_e['PS']) | (e['NS'] & del_e['NS']) | (e['NS'] & del_e['ZO']) | (e['ZO'] & del_e['NS']) | (e['PB'] & del_e['NS']) | (e['PB'] & del_e['ZO']) | (e['PB'] & del_e['PS']), kp['NS'])
    rule3 = ctrl.Rule((e['NB'] & del_e['PB']) | (e['NS'] & del_e['PS']) | (e['ZO'] & del_e['ZO']) | (e['PS'] & del_e['NS']) | (e['PB'] & del_e['NB']), kp['ZO'])
    rule4 = ctrl.Rule((e['NS'] & del_e['NB']) | (e['NS'] & del_e['PB']) | (e['ZO'] & del_e['NB']) | (e['ZO'] & del_e['PS']) | (e['ZO'] & del_e['PB']) | (e['PS'] & del_e['NB']) | (e['PS'] & del_e['ZO']) | (e['PS'] & del_e['PS']), kp['PS'])
    rule5 = ctrl.Rule((e['PB'] & del_e['PB']), kp['NB'])

    rule6 = ctrl.Rule((e['PS'] & del_e['PB']) | (e['PB'] & del_e['PB']), ki['PB'])
    rule7 = ctrl.Rule((e['NB'] & del_e['NS']) | (e['NB'] & del_e['ZO']) | (e['NB'] & del_e['PS']) | (e['NS'] & del_e['NS']) | (e['NS'] & del_e['ZO'])| (e['ZO'] & del_e['NB']) | (e['ZO'] & del_e['NS']) | (e['PS'] & del_e['NB']), ki['NS'])
    rule8 = ctrl.Rule((e['NB'] & del_e['PB']) | (e['NS'] & del_e['PS']) | (e['ZO'] & del_e['ZO']) | (e['PS'] & del_e['NS']) | (e['PB'] & del_e['NB']), ki['ZO'])
    rule9 = ctrl.Rule((e['NS'] & del_e['PB']) | (e['ZO'] & del_e['PS']) | (e['ZO'] & del_e['PB']) | (e['PS'] & del_e['ZO']) | (e['PS'] & del_e['PS']) | (e['PB'] & del_e['NS']) | (e['PB'] & del_e['ZO'])| (e['PB'] & del_e['PS']), ki['PS'])
    rule10 = ctrl.Rule((e['NB'] & del_e['NB'])|(e['NS'] & del_e['NB']), ki['NB'])

    rule11 = ctrl.Rule((e['PB'] & del_e['NB']) | (e['PB'] & del_e['PB']), kd['PB'])
    rule12 = ctrl.Rule((e['NS'] & del_e['NS']) | (e['NS'] & del_e['ZO']) | (e['NS'] & del_e['PS']) | (e['ZO'] & del_e['NS']) | (e['ZO'] & del_e['ZO'])| (e['ZO'] & del_e['PS']), kd['NS'])
    rule13 = ctrl.Rule((e['NS'] & del_e['NB']) | (e['NS'] & del_e['PB']) | (e['ZO'] & del_e['NB'])| (e['ZO'] & del_e['PB']) | (e['PS'] & del_e['NB']) | (e['PS'] & del_e['NS'])| (e['PS'] & del_e['ZO'])| (e['PS'] & del_e['PS'])| (e['PS'] & del_e['PB']), kd['ZO'])
    rule14 = ctrl.Rule((e['NS'] & del_e['PB']) | (e['ZO'] & del_e['PS']) | (e['ZO'] & del_e['PB']) | (e['PS'] & del_e['ZO']) | (e['PS'] & del_e['PS']) | (e['PB'] & del_e['NS']) | (e['PB'] & del_e['ZO'])| (e['PB'] & del_e['PS']), kd['PS'])
    rule15 = ctrl.Rule((e['NB'] & del_e['NS'])|(e['NB'] & del_e['ZO'])|(e['NB'] & del_e['PS']), kd['NB'])

    sys = ctrl.ControlSystem([rule1, rule2, rule3, rule4, rule5,rule6, rule7, rule8, rule9, rule10, rule11, rule12, rule13, rule14, rule15])
    fu = ctrl.ControlSystemSimulation(sys)

    for _ in range(num_iterations):
        error = system.get_error()
        fu.input['e']=error
        fu.input['del_e']= error-controller.last_error
        fu.compute()
        controller.Kp += fu.output['kp']
        controller.Ki += fu.output['ki']
        controller.Kd += fu.output['kd']
        print("KP:",controller.Kp,"KI:",controller.Ki,"KD:",controller.Kd,"error:",error,"del_error:",error-controller.last_error)
        control_signal = controller.control(error)
        # system.update(control_signal)
        temp.append(system.update(control_signal))
        errors.append(error)
        signal.append(control_signal)
    return temp

#controller.input['e']=20
#controller.input['del_e']=2
#controller.compute()
#p=controller.output['kp']
#print(p)

#upsampled = np.linspace(-10,10,21)
#x,y=np.meshgrid(upsampled,upsampled)
#z=np.zeros_like(x)
#pp=[]
#for i in range(0,10):
#    for j in range(-10,10):
#        controller.input['e']=x[i,j]
#        controller.input['del_e']=y[i,j]
#        controller.compute()
#        z[i,j]=controller.output['kp']
#        pp.append(z[i,j])

#fig = plt.figure(figsize=(10,10))
#ax = fig.add_subplot(111,projection='3d')
#surf = ax.plot_surface(x,y,z,rstride=1,cstride=1,cmap='viridis',linewidth=0.4,antialiased=True)
#ax.view_init(30,200)
#plt.show()



target_temp = 30
pid_controller = PIDController(0.4,0,0)
temperature_system = TemperatureSystem(target_temp)
temp = train(pid_controller,temperature_system, 10)
plt.plot(temp, 'ro--', linewidth=2, markersize=6, label="fuzzy")  # 簡化後的程式碼
plt.xlabel('Time')
plt.ylabel('temp')
plt.title('ML PID control')
plt.legend()
plt.show()