python/test/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 sys.path.insert(0, path)
   7 if len(sys.argv) > 1:
   8     sys.stdout = open(sys.argv[1], "w")
   9
  10 import liba  # type: ignore
  11
  12 X = 1
  13 NL = -3 * X
  14 NM = -2 * X
  15 NS = -1 * X
  16 ZO = +0 * X
  17 PS = +1 * X
  18 PM = +2 * X
  19 PL = +3 * X
  20 me = [
  21     [liba.mf.TRI, NL, NL, NM],
  22     [liba.mf.TRI, NL, NM, NS],
  23     [liba.mf.TRI, NM, NS, ZO],
  24     [liba.mf.TRI, NS, ZO, PS],
  25     [liba.mf.TRI, ZO, PS, PM],
  26     [liba.mf.TRI, PS, PM, PL],
  27     [liba.mf.TRI, PM, PL, PL],
  28 ]
  29 NL = -6 * X
  30 NM = -4 * X
  31 NS = -2 * X
  32 ZO = +0 * X
  33 PS = +2 * X
  34 PM = +4 * X
  35 PL = +6 * X
  36 mec = [
  37     [liba.mf.TRI, NL, NL, NM],
  38     [liba.mf.TRI, NL, NM, NS],
  39     [liba.mf.TRI, NM, NS, ZO],
  40     [liba.mf.TRI, NS, ZO, PS],
  41     [liba.mf.TRI, ZO, PS, PM],
  42     [liba.mf.TRI, PS, PM, PL],
  43     [liba.mf.TRI, PM, PL, PL],
  44 ]
  45 X = 10 / 3
  46 NL = -3 * X
  47 NM = -2 * X
  48 NS = -1 * X
  49 ZO = +0 * X
  50 PS = +1 * X
  51 PM = +2 * X
  52 PL = +3 * X
  53 mkp = [
  54     [NL, NL, NM, NM, NS, ZO, ZO],
  55     [NL, NL, NM, NS, NS, ZO, PS],
  56     [NM, NM, NM, NS, ZO, PS, PS],
  57     [NM, NM, NS, ZO, PS, PM, PM],
  58     [NS, NS, ZO, PS, PS, PM, PM],
  59     [NS, ZO, PS, PM, PM, PM, PL],
  60     [ZO, ZO, PM, PM, PM, PL, PL],
  61 ]
  62 X = 0.01 / 3
  63 NL = -3 * X
  64 NM = -2 * X
  65 NS = -1 * X
  66 ZO = +0 * X
  67 PS = +1 * X
  68 PM = +2 * X
  69 PL = +3 * X
  70 mki = [
  71     [PL, PL, PM, PM, PS, ZO, ZO],
  72     [PL, PL, PM, PS, PS, ZO, ZO],
  73     [PL, PM, PS, PS, ZO, NS, NS],
  74     [PM, PM, PS, ZO, NS, NM, NM],
  75     [PM, PS, ZO, NS, NS, NM, NL],
  76     [ZO, ZO, NS, NS, NM, NL, NL],
  77     [ZO, ZO, NS, NM, NM, NL, NL],
  78 ]
  79 X = 0.1 / 3
  80 NL = -3 * X
  81 NM = -2 * X
  82 NS = -1 * X
  83 ZO = +0 * X
  84 PS = +1 * X
  85 PM = +2 * X
  86 PL = +3 * X
  87 mkd = [
  88     [NS, PS, PL, PL, PL, PM, NS],
  89     [NS, PS, PL, PM, PM, PS, ZO],
  90     [ZO, PS, PM, PM, PS, PS, ZO],
  91     [ZO, PS, PS, PS, PS, PS, ZO],
  92     [ZO, ZO, ZO, ZO, ZO, ZO, ZO],
  93     [NL, NS, NS, NS, NS, NS, NL],
  94     [NL, NM, NM, NM, NS, NS, NL],
  95 ]
  96
  97 Ts = 0.001
  98 MIN = -10
  99 MAX = +10
 100
 101 kp = 9
 102 ki = 0.01 * Ts
 103 kd = 0.1 / Ts
 104
 105 num = [6.59492796e-05, 6.54019884e-05]
 106 den = [1.0, -1.97530991, 0.97530991]
 107
 108 tf = liba.tf(num, den[1:])
 109 pid_fuzzy = (
 110     liba.pid_fuzzy().rule(me, mec, mkp, mki, mkd).set_block(2).op(liba.pid_fuzzy.CAP_ALGEBRA)
 111 )
 112 pid_fuzzy.outmax = MAX
 113 pid_fuzzy.outmin = MIN
 114
 115 t = 0
 116 x = []
 117 while t <= 0.2:
 118     x.append(t)
 119     t += Ts
 120 N = len(x)
 121 r = 1
 122
 123 y1 = [0.0] * N
 124 for i in range(len(x)):
 125     y1[i] = tf(pid_fuzzy.pos(r, tf.output[0]))
 126
 127 pid_fuzzy.zero()
 128 tf.zero()
 129
 130 y2 = [0.0] * N
 131 for i in range(len(x)):
 132     y2[i] = tf(pid_fuzzy.inc(r, tf.output[0]))
 133
 134 for i in range(len(x)):
 135     print("%g,%g,%g,%g" % (x[i], r, y1[i], y2[i]))
 136
 137 pid_fuzzy.me
 138 pid_fuzzy.mec
 139 pid_fuzzy.mkp
 140 pid_fuzzy.mki
 141 pid_fuzzy.mkd
 142 pid_fuzzy.kp = pid_fuzzy.kp
 143 pid_fuzzy.ki = pid_fuzzy.ki
 144 pid_fuzzy.kd = pid_fuzzy.kd
 145 pid_fuzzy.summax = pid_fuzzy.summax
 146 pid_fuzzy.summin = pid_fuzzy.summin
 147 pid_fuzzy.sum
 148 pid_fuzzy.outmax = pid_fuzzy.outmax
 149 pid_fuzzy.outmin = pid_fuzzy.outmin
 150 pid_fuzzy.out
 151 pid_fuzzy.fdb
 152 pid_fuzzy.err
 153 pid_fuzzy.me
 154 pid_fuzzy.mec
 155 pid_fuzzy.mkp
 156 pid_fuzzy.mki
 157 pid_fuzzy.mkd
 158 pid_fuzzy.order
 159 pid_fuzzy.block = pid_fuzzy.block + 1