python/test/3rd/pid_fuzzy.py

   1 #!/usr/bin/env python
   2 import os, sys
   3
   4 base = os.path.dirname(__file__)
   5 path = os.path.dirname(base)
   6 path = os.path.dirname(path)
   7 sys.path.insert(0, path)
   8 try:
   9     import numpy as np
  10     import matplotlib.pyplot as plt
  11 except Exception as e:
  12     print(e)
  13     exit()
  14
  15 import liba  # type: ignore
  16
  17 X = 1
  18 NL = -3 * X
  19 NM = -2 * X
  20 NS = -1 * X
  21 ZO = +0 * X
  22 PS = +1 * X
  23 PM = +2 * X
  24 PL = +3 * X
  25 me = [
  26     liba.new_num([liba.mf.TRI, NL, NL, NM]),
  27     liba.new_num([liba.mf.TRI, NL, NM, NS]),
  28     liba.new_num([liba.mf.TRI, NM, NS, ZO]),
  29     liba.new_num([liba.mf.TRI, NS, ZO, PS]),
  30     liba.new_num([liba.mf.TRI, ZO, PS, PM]),
  31     liba.new_num([liba.mf.TRI, PS, PM, PL]),
  32     liba.new_num([liba.mf.TRI, PM, PL, PL]),
  33 ]
  34 NL = -6 * X
  35 NM = -4 * X
  36 NS = -2 * X
  37 ZO = +0 * X
  38 PS = +2 * X
  39 PM = +4 * X
  40 PL = +6 * X
  41 mec = [
  42     liba.new_num([liba.mf.TRI, NL, NL, NM]),
  43     liba.new_num([liba.mf.TRI, NL, NM, NS]),
  44     liba.new_num([liba.mf.TRI, NM, NS, ZO]),
  45     liba.new_num([liba.mf.TRI, NS, ZO, PS]),
  46     liba.new_num([liba.mf.TRI, ZO, PS, PM]),
  47     liba.new_num([liba.mf.TRI, PS, PM, PL]),
  48     liba.new_num([liba.mf.TRI, PM, PL, PL]),
  49 ]
  50 X = 10 / 3
  51 NL = -3 * X
  52 NM = -2 * X
  53 NS = -1 * X
  54 ZO = +0 * X
  55 PS = +1 * X
  56 PM = +2 * X
  57 PL = +3 * X
  58 mkp = [
  59     [NL, NL, NM, NM, NS, ZO, ZO],
  60     [NL, NL, NM, NS, NS, ZO, PS],
  61     [NM, NM, NM, NS, ZO, PS, PS],
  62     [NM, NM, NS, ZO, PS, PM, PM],
  63     [NS, NS, ZO, PS, PS, PM, PM],
  64     [NS, ZO, PS, PM, PM, PM, PL],
  65     [ZO, ZO, PM, PM, PM, PL, PL],
  66 ]
  67 X = 0.01 / 3
  68 NL = -3 * X
  69 NM = -2 * X
  70 NS = -1 * X
  71 ZO = +0 * X
  72 PS = +1 * X
  73 PM = +2 * X
  74 PL = +3 * X
  75 mki = [
  76     [PL, PL, PM, PM, PS, ZO, ZO],
  77     [PL, PL, PM, PS, PS, ZO, ZO],
  78     [PL, PM, PS, PS, ZO, NS, NS],
  79     [PM, PM, PS, ZO, NS, NM, NM],
  80     [PM, PS, ZO, NS, NS, NM, NL],
  81     [ZO, ZO, NS, NS, NM, NL, NL],
  82     [ZO, ZO, NS, NM, NM, NL, NL],
  83 ]
  84 X = 0.1 / 3
  85 NL = -3 * X
  86 NM = -2 * X
  87 NS = -1 * X
  88 ZO = +0 * X
  89 PS = +1 * X
  90 PM = +2 * X
  91 PL = +3 * X
  92 mkd = [
  93     [NS, PS, PL, PL, PL, PM, NS],
  94     [NS, PS, PL, PM, PM, PS, ZO],
  95     [ZO, PS, PM, PM, PS, PS, ZO],
  96     [ZO, PS, PS, PS, PS, PS, ZO],
  97     [ZO, ZO, ZO, ZO, ZO, ZO, ZO],
  98     [NL, NS, NS, NS, NS, NS, NL],
  99     [NL, NM, NM, NM, NS, NS, NL],
 100 ]
 101
 102
 103 def fuzzy(e: float, ec: float):
 104     idx_e = []
 105     val_e = []
 106     for idx, param in enumerate(me):
 107         ms = liba.mf.mf(param[0], e, param[1:])
 108         if ms > 0:
 109             idx_e.append(idx)
 110             val_e.append(ms)
 111     if idx_e == []:
 112         return 0, 0, 0
 113
 114     idx_ec = []
 115     val_ec = []
 116     for idx, param in enumerate(mec):
 117         ms = liba.mf.mf(param[0], ec, param[1:])
 118         if ms > 0:
 119             idx_ec.append(idx)
 120             val_ec.append(ms)
 121     if idx_ec == []:
 122         return 0, 0, 0
 123
 124     inv = 0
 125     mat = []
 126     for e in val_e:
 127         row = []
 128         for ec in val_ec:
 129             y = (e * ec) ** 0.5 * (1 - (1 - e) * (1 - ec)) ** 0.5
 130             row.append(y)
 131             inv += y
 132         mat.append(row)
 133     inv = 1 / inv
 134
 135     kp = 0.0
 136     for i in range(len(val_e)):
 137         for j in range(len(val_ec)):
 138             kp += mat[i][j] * mkp[idx_e[i]][idx_ec[j]]
 139     kp *= inv
 140     ki = 0.0
 141     for i in range(len(val_e)):
 142         for j in range(len(val_ec)):
 143             ki += mat[i][j] * mki[idx_e[i]][idx_ec[j]]
 144     ki *= inv
 145     kd = 0.0
 146     for i in range(len(val_e)):
 147         for j in range(len(val_ec)):
 148             kd += mat[i][j] * mkd[idx_e[i]][idx_ec[j]]
 149     kd *= inv
 150
 151     return kp, ki, kd
 152
 153
 154 Ts = 0.001
 155 kp = 9
 156 ki = 0.01 * Ts
 157 kd = 0.1 / Ts
 158 data = np.arange(0, 0.2, Ts)
 159
 160 try:
 161     import control.matlab as ct
 162
 163     sysc = ct.tf(133, [1, 25, 0])
 164     sysd = ct.c2d(sysc, Ts)
 165     print(sysc, sysd)
 166
 167     [[num]], [[den]] = ct.tfdata(sysd)
 168 except ModuleNotFoundError:
 169     num = [6.59492796e-05, 6.54019884e-05]
 170     den = [1.0, -1.97530991, 0.97530991]
 171 except Exception as e:
 172     print(e)
 173     exit()
 174
 175 MIN = -10
 176 MAX = +10
 177 tf = liba.tf(num, den[1:])
 178 pid_fuzzy = (
 179     liba.pid_fuzzy()
 180     .set_opr(liba.pid_fuzzy.CAP_ALGEBRA)
 181     .set_rule(me, mec, mkp, mki, mkd)
 182     .set_nfuzz(2)
 183 )
 184 pid_fuzzy.outmax = MAX
 185 pid_fuzzy.outmin = MIN
 186
 187 r = 1.0
 188 setpoint = [r] * len(data)
 189
 190 title = "Fuzzy Proportional Integral Derivative"
 191
 192 y = 0.0
 193 tf.zero()
 194 error1 = []
 195 feedback1 = []
 196 pid_fuzzy.set_kpid(kp, ki, kd)
 197 for i in data:
 198     y = pid_fuzzy.inc(r, y)
 199     y = tf(y)
 200     feedback1.append(y)
 201     error1.append(r - y)
 202
 203 u = 0.0
 204 y = 0.0
 205 tf.zero()
 206 e = [0.0, 0.0, 0.0]
 207 x = [0.0, 0.0, 0.0]
 208 error2 = []
 209 feedback2 = []
 210 for i in data:
 211     e = np.roll(e, 1)
 212     e[0] = r - y
 213     x[0] = e[0] - e[1]
 214     x[1] = e[0]
 215     x[2] = e[0] - e[1] * 2 + e[2]
 216     dkp, dki, dkd = fuzzy(e[0], e[0] - e[1])
 217     u += (kp + dkp) * x[0] + (ki + dki) * x[1] + (kd + dkd) * x[2]
 218     y = u
 219     if y < MIN:
 220         y = MIN
 221     elif y > MAX:
 222         y = MAX
 223     y = tf(y)
 224     feedback2.append(y)
 225     error2.append(r - y)
 226
 227 plt.figure(title)
 228 plt.subplot(211)
 229 plt.title(title)
 230 plt.plot(data, setpoint, "r-", data, feedback1, "b-", data, feedback2, "g-")
 231 plt.ylabel("r - y")
 232 plt.grid(True)
 233 plt.subplot(212)
 234 plt.plot(data, error1, "b-", data, error2, "g-")
 235 plt.ylabel("error")
 236 plt.xlabel("time(s)")
 237 plt.grid(True)
 238 plt.savefig(os.path.join(base, "pid_fuzzy.png"))
 239 plt.show()