import os, sys
base = os.path.dirname(__file__)
path = os.path.dirname(base)
path = os.path.dirname(path)
sys.path.insert(0, path)
try:
import numpy as np
import matplotlib.pyplot as plt
except Exception as e:
print(e)
exit()
import liba
X = 1
NL = -3 * X
NM = -2 * X
NS = -1 * X
ZO = +0 * X
PS = +1 * X
PM = +2 * X
PL = +3 * X
me = [
liba.new_num([liba.mf.TRI, NL, NL, NM]),
liba.new_num([liba.mf.TRI, NL, NM, NS]),
liba.new_num([liba.mf.TRI, NM, NS, ZO]),
liba.new_num([liba.mf.TRI, NS, ZO, PS]),
liba.new_num([liba.mf.TRI, ZO, PS, PM]),
liba.new_num([liba.mf.TRI, PS, PM, PL]),
liba.new_num([liba.mf.TRI, PM, PL, PL]),
]
NL = -6 * X
NM = -4 * X
NS = -2 * X
ZO = +0 * X
PS = +2 * X
PM = +4 * X
PL = +6 * X
mec = [
liba.new_num([liba.mf.TRI, NL, NL, NM]),
liba.new_num([liba.mf.TRI, NL, NM, NS]),
liba.new_num([liba.mf.TRI, NM, NS, ZO]),
liba.new_num([liba.mf.TRI, NS, ZO, PS]),
liba.new_num([liba.mf.TRI, ZO, PS, PM]),
liba.new_num([liba.mf.TRI, PS, PM, PL]),
liba.new_num([liba.mf.TRI, PM, PL, PL]),
]
X = 10 / 3
NL = -3 * X
NM = -2 * X
NS = -1 * X
ZO = +0 * X
PS = +1 * X
PM = +2 * X
PL = +3 * X
mkp = [
[NL, NL, NM, NM, NS, ZO, ZO],
[NL, NL, NM, NS, NS, ZO, PS],
[NM, NM, NM, NS, ZO, PS, PS],
[NM, NM, NS, ZO, PS, PM, PM],
[NS, NS, ZO, PS, PS, PM, PM],
[NS, ZO, PS, PM, PM, PM, PL],
[ZO, ZO, PM, PM, PM, PL, PL],
]
X = 0.01 / 3
NL = -3 * X
NM = -2 * X
NS = -1 * X
ZO = +0 * X
PS = +1 * X
PM = +2 * X
PL = +3 * X
mki = [
[PL, PL, PM, PM, PS, ZO, ZO],
[PL, PL, PM, PS, PS, ZO, ZO],
[PL, PM, PS, PS, ZO, NS, NS],
[PM, PM, PS, ZO, NS, NM, NM],
[PM, PS, ZO, NS, NS, NM, NL],
[ZO, ZO, NS, NS, NM, NL, NL],
[ZO, ZO, NS, NM, NM, NL, NL],
]
X = 0.1 / 3
NL = -3 * X
NM = -2 * X
NS = -1 * X
ZO = +0 * X
PS = +1 * X
PM = +2 * X
PL = +3 * X
mkd = [
[NS, PS, PL, PL, PL, PM, NS],
[NS, PS, PL, PM, PM, PS, ZO],
[ZO, PS, PM, PM, PS, PS, ZO],
[ZO, PS, PS, PS, PS, PS, ZO],
[ZO, ZO, ZO, ZO, ZO, ZO, ZO],
[NL, NS, NS, NS, NS, NS, NL],
[NL, NM, NM, NM, NS, NS, NL],
]
def fuzzy(e: float, ec: float):
idx_e = []
val_e = []
for idx, param in enumerate(me):
ms = liba.mf.mf(param[0], e, param[1:])
if ms > 0:
idx_e.append(idx)
val_e.append(ms)
if idx_e == []:
return 0, 0, 0
idx_ec = []
val_ec = []
for idx, param in enumerate(mec):
ms = liba.mf.mf(param[0], ec, param[1:])
if ms > 0:
idx_ec.append(idx)
val_ec.append(ms)
if idx_ec == []:
return 0, 0, 0
inv = 0
mat = []
for e in val_e:
row = []
for ec in val_ec:
y = (e * ec) ** 0.5 * (1 - (1 - e) * (1 - ec)) ** 0.5
row.append(y)
inv += y
mat.append(row)
inv = 1 / inv
kp = 0.0
for i in range(len(val_e)):
for j in range(len(val_ec)):
kp += mat[i][j] * mkp[idx_e[i]][idx_ec[j]]
kp *= inv
ki = 0.0
for i in range(len(val_e)):
for j in range(len(val_ec)):
ki += mat[i][j] * mki[idx_e[i]][idx_ec[j]]
ki *= inv
kd = 0.0
for i in range(len(val_e)):
for j in range(len(val_ec)):
kd += mat[i][j] * mkd[idx_e[i]][idx_ec[j]]
kd *= inv
return kp, ki, kd
Ts = 0.001
kp = 9
ki = 0.01 * Ts
kd = 0.1 / Ts
data = np.arange(0, 0.2, Ts)
try:
import control.matlab as ct
sysc = ct.tf(133, [1, 25, 0])
sysd = ct.c2d(sysc, Ts)
print(sysc, sysd)
[[num]], [[den]] = ct.tfdata(sysd)
except ModuleNotFoundError:
num = [6.59492796e-05, 6.54019884e-05]
den = [1.0, -1.97530991, 0.97530991]
except Exception as e:
print(e)
exit()
MIN = -10
MAX = +10
tf = liba.tf(num, den[1:])
pid_fuzzy = (
liba.pid_fuzzy()
.set_opr(liba.pid_fuzzy.CAP_ALGEBRA)
.set_rule(me, mec, mkp, mki, mkd)
.set_nfuzz(2)
)
pid_fuzzy.outmax = MAX
pid_fuzzy.outmin = MIN
r = 1.0
setpoint = [r] * len(data)
title = "Fuzzy Proportional Integral Derivative"
y = 0.0
tf.zero()
error1 = []
feedback1 = []
pid_fuzzy.set_kpid(kp, ki, kd)
for i in data:
y = pid_fuzzy.inc(r, y)
y = tf(y)
feedback1.append(y)
error1.append(r - y)
u = 0.0
y = 0.0
tf.zero()
e = [0.0, 0.0, 0.0]
x = [0.0, 0.0, 0.0]
error2 = []
feedback2 = []
for i in data:
e = np.roll(e, 1)
e[0] = r - y
x[0] = e[0] - e[1]
x[1] = e[0]
x[2] = e[0] - e[1] * 2 + e[2]
dkp, dki, dkd = fuzzy(e[0], e[0] - e[1])
u += (kp + dkp) * x[0] + (ki + dki) * x[1] + (kd + dkd) * x[2]
y = u
if y < MIN:
y = MIN
elif y > MAX:
y = MAX
y = tf(y)
feedback2.append(y)
error2.append(r - y)
plt.figure(title)
plt.subplot(211)
plt.title(title)
plt.plot(data, setpoint, "r-", data, feedback1, "b-", data, feedback2, "g-")
plt.ylabel("r - y")
plt.grid(True)
plt.subplot(212)
plt.plot(data, error1, "b-", data, error2, "g-")
plt.ylabel("error")
plt.xlabel("time(s)")
plt.grid(True)
plt.savefig(os.path.join(base, "pid_fuzzy.png"))
plt.show()