python/test/pid.py

   1 #!/usr/bin/env python
   2 from __future__ import print_function
   3 import os, sys
   4
   5 path = os.path.dirname(__file__)
   6 path = os.path.dirname(path)
   7 sys.path.insert(0, path)
   8 if len(sys.argv) > 1:
   9     sys.stdout = open(sys.argv[1], "w")
  10
  11 import liba  # type: ignore
  12
  13 Ts = 0.001
  14 MIN = -10
  15 MAX = +10
  16
  17 kp = 10.0
  18 ki = 0.01 * Ts
  19 kd = 0.24 / Ts
  20
  21 num = [6.59492796e-05, 6.54019884e-05]
  22 den = [1.0, -1.97530991, 0.97530991]
  23
  24 tf = liba.tf(num, den[1:])
  25 pid = liba.pid().set_kpid(kp, ki, kd)
  26 pid.outmax = MAX
  27 pid.outmin = MIN
  28
  29 t = 0
  30 x = []
  31 while t <= 0.2:
  32     x.append(t)
  33     t += Ts
  34 N = len(x)
  35 r = 1
  36
  37 y1 = liba.new_num(N)
  38 for i in range(len(x)):
  39     y1[i] = tf(pid.pos(r, tf.output[0]))
  40
  41 pid.zero()
  42 tf.zero()
  43
  44 y2 = liba.new_num(N)
  45 for i in range(len(x)):
  46     y2[i] = tf(pid.inc(r, tf.output[0]))
  47
  48 for i in range(len(x)):
  49     print("%g,%g,%g,%g" % (x[i], r, y1[i], y2[i]))
  50
  51 pid.kp = pid.kp
  52 pid.ki = pid.ki
  53 pid.kd = pid.kd
  54 pid.summax = pid.summax
  55 pid.summin = pid.summin
  56 pid.sum
  57 pid.outmax = pid.outmax
  58 pid.outmin = pid.outmin
  59 pid.out
  60 pid.fdb
  61 pid.err