python/test/pid.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 Ts = 0.001
  13 MIN = -10
  14 MAX = +10
  15
  16 kp = 10.0
  17 ki = 0.01 * Ts
  18 kd = 0.24 / Ts
  19
  20 num = [6.59492796e-05, 6.54019884e-05]
  21 den = [1.0, -1.97530991, 0.97530991]
  22
  23 tf = liba.tf(num, den[1:])
  24 pid = liba.pid().kpid(kp, ki, kd)
  25 pid.outmax = MAX
  26 pid.outmin = MIN
  27
  28 t = 0
  29 x = []
  30 while t <= 0.2:
  31     x.append(t)
  32     t += Ts
  33 N = len(x)
  34 r = 1
  35
  36 y1 = [0.0] * N
  37 for i in range(len(x)):
  38     y1[i] = tf(pid.pos(r, tf.output[0]))
  39
  40 pid.zero()
  41 tf.zero()
  42
  43 y2 = [0.0] * N
  44 for i in range(len(x)):
  45     y2[i] = tf(pid.inc(r, tf.output[0]))
  46
  47 for i in range(len(x)):
  48     print("%g,%g,%g,%g" % (x[i], r, y1[i], y2[i]))
  49
  50 pid.kp = pid.kp
  51 pid.ki = pid.ki
  52 pid.kd = pid.kd
  53 pid.summax = pid.summax
  54 pid.summin = pid.summin
  55 pid.sum
  56 pid.outmax = pid.outmax
  57 pid.outmin = pid.outmin
  58 pid.out
  59 pid.fdb
  60 pid.err