python/test/pid_fuzzy.py

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