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