python/test/3rd/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 path = os.path.dirname(path)
   7 sys.path.insert(0, path)
   8 try:
   9     import numpy as np
  10     import matplotlib.pyplot as plt
  11 except Exception as e:
  12     print(e)
  13     exit()
  14
  15 Ts = 0.001
  16 try:
  17     import control.matlab as ct
  18
  19     sysc = ct.tf(133, [1, 25, 0])
  20     sysd = ct.c2d(sysc, Ts)
  21     print(sysc, sysd)
  22
  23     [[num]], [[den]] = ct.tfdata(sysd)
  24 except ModuleNotFoundError:
  25     num = [6.59492796e-05, 6.54019884e-05]
  26     den = [1.0, -1.97530991, 0.97530991]
  27 except Exception as e:
  28     print(e)
  29     exit()
  30 import liba  # type: ignore
  31
  32 MIN = -10
  33 MAX = +10
  34
  35 kp = 10.0
  36 ki = 0.01 * Ts
  37 kd = 0.24 / Ts
  38
  39 tf = liba.tf(num, den[1:])
  40 pid = liba.pid().kpid(kp, ki, kd)
  41 pid.outmax = MAX
  42 pid.outmin = MIN
  43
  44 r = 1.0
  45 data = np.arange(0, 0.2, Ts)
  46 setpoint = [r] * len(data)
  47
  48 title = "Proportional Integral Derivative Position"
  49
  50 y = 0.0
  51 tf.zero()
  52 error1 = []
  53 feedback1 = []
  54 pid.zero()
  55 for i in data:
  56     u = pid.pos(r, y)
  57     y = tf(u)
  58     feedback1.append(y)
  59     error1.append(r - y)
  60
  61 y = 0.0
  62 tf.zero()
  63 error2 = []
  64 feedback2 = []
  65 e = [0.0, 0.0, 0.0]
  66 x = [0.0, 0.0, 0.0]
  67 for i in data:
  68     e = np.roll(e, 1)
  69     e[0] = r - y
  70     x[0] = e[0]
  71     x[1] += ki * e[0]
  72     x[2] = e[0] - e[1]
  73     u = kp * x[0] + x[1] + kd * x[2]
  74     if u < MIN:
  75         u = MIN
  76     elif u > MAX:
  77         u = MAX
  78     y = tf(u)
  79     feedback2.append(y)
  80     error2.append(r - y)
  81
  82 plt.figure(title)
  83 plt.subplot(211)
  84 plt.title(title)
  85 plt.plot(data, setpoint, "r-", data, feedback1, "b-", data, feedback2, "g-")
  86 plt.ylabel("r - y")
  87 plt.grid(True)
  88 plt.subplot(212)
  89 plt.plot(data, error1, "b-", data, error2, "g-")
  90 plt.ylabel("error")
  91 plt.xlabel("time(s)")
  92 plt.grid(True)
  93 plt.savefig(os.path.join(base, "pid_pos.png"))
  94
  95 title = "Proportional Integral Derivative Increment"
  96
  97 y = 0.0
  98 tf.zero()
  99 error1 = []
 100 feedback1 = []
 101 pid.zero()
 102 for i in data:
 103     u = pid.inc(r, y)
 104     y = tf(u)
 105     feedback1.append(y)
 106     error1.append(r - y)
 107
 108 u = 0.0
 109 y = 0.0
 110 tf.zero()
 111 error2 = []
 112 feedback2 = []
 113 e = [0.0, 0.0, 0.0]
 114 x = [0.0, 0.0, 0.0]
 115 for i in data:
 116     e = np.roll(e, 1)
 117     e[0] = r - y
 118     x[0] = e[0] - e[1]
 119     x[1] = e[0]
 120     x[2] = e[0] - e[1] * 2 + e[2]
 121     u += kp * x[0] + ki * x[1] + kd * x[2]
 122     y = u
 123     if y < MIN:
 124         y = MIN
 125     elif y > MAX:
 126         y = MAX
 127     y = tf(y)
 128     feedback2.append(y)
 129     error2.append(r - y)
 130
 131 plt.figure(title)
 132 plt.subplot(211)
 133 plt.title(title)
 134 plt.plot(data, setpoint, "r-", data, feedback1, "b-", data, feedback2, "g-")
 135 plt.ylabel("r - y")
 136 plt.grid(True)
 137 plt.subplot(212)
 138 plt.plot(data, error1, "b-", data, error2, "g-")
 139 plt.ylabel("error")
 140 plt.xlabel("time(s)")
 141 plt.grid(True)
 142 plt.savefig(os.path.join(base, "pid_inc.png"))
 143
 144 plt.show()