python/test/pid_neuro.py

   1 #!/usr/bin/env python
   2 import os, sys
   3
   4 sys.path.insert(0, os.getcwd())
   5 prefix = os.path.join(sys.path[0], "build")
   6 if not os.path.exists(prefix):
   7     os.mkdir(prefix)
   8 try:
   9     import liba as a
  10     import numpy as np
  11     import matplotlib.pyplot as plt
  12 except Exception as e:
  13     print(e)
  14     exit()
  15
  16 Ts = 0.001
  17 k = 4000
  18 kp = 4
  19 ki = 0.04
  20 kd = 1.0
  21 wp = 1
  22 wi = 0
  23 wd = 0
  24 data = np.arange(0, 0.1, Ts)
  25
  26 try:
  27     import control.matlab as ct
  28
  29     sysc = ct.tf(133, [1, 25, 0])
  30     sysd = ct.c2d(sysc, Ts)
  31     print(sysc, sysd)
  32
  33     [[num]], [[den]] = ct.tfdata(sysd)
  34 except ModuleNotFoundError:
  35     num = [6.59492796e-05, 6.54019884e-05]
  36     den = [1.0, -1.97530991, 0.97530991]
  37 except Exception as e:
  38     print(e)
  39     exit()
  40
  41 tf = a.tf(num, den[1:])
  42 pid_neuro = a.pid_neuro()
  43
  44 r = 1.0
  45 setpoint = [r] * len(data)
  46
  47 title = "Single Neuron Proportional Integral Derivative"
  48
  49 y = 0.0
  50 tf.zero()
  51 error1 = []
  52 feedback1 = []
  53 pid_neuro.kpid(k, kp, ki, kd).wpid(wp, wi, wd)
  54 for i in data:
  55     y = pid_neuro.inc(r, y)
  56     y = tf(y)
  57     feedback1.append(y)
  58     error1.append(r - y)
  59
  60 plt.figure(title)
  61 plt.subplot(211)
  62 plt.title(title)
  63 plt.plot(data, setpoint, "r-", data, feedback1, "b-")
  64 plt.ylabel("r - y")
  65 plt.grid(True)
  66 plt.subplot(212)
  67 plt.plot(data, error1, "b-")
  68 plt.ylabel("error")
  69 plt.xlabel("time(s)")
  70 plt.grid(True)
  71 plt.savefig(os.path.join(prefix, "pid_neuro.png"))