<SYSTEM_TASK:>
Given the following text description, write Python code to implement the functionality described below step by step
<END_TASK>
<USER_TASK:>
Description:
 RST design for a DC-motor
From HW3 2017 spring semester
Plant model
Zero-order-hold sampling of the DC-motor with transfer function
  $$ G(s) = \frac{1}{s(2s+1)} $$
  gives a discrete-time system with pulse transfer function 
  \begin{equation}
  G_d(z) = \frac{B(z)}{A(z)}= \frac{2\big(\frac{h}{2}-1+\mathrm{e}^{-\frac{h}{2}}\big)z + 2\big(1-\mathrm{e}^{-\frac{h}{2}}-\frac{h}{2}\mathrm{e}^{-\frac{h}{2}}\big)}{(z-1)\big(z-\mathrm{e}^{-\frac{h}{2}}\big)}.
  \end{equation}
Step1:  Determine sampling period and desired closed loop poles
In a continuous-time description of the desired closed-loop system we want the system to have two dominating poles at
   $$ -5 \pm i5. $$
   In addition to the two dominating poles, we want a third pole at
   $$ a=-20 $$
   to be able to control the response to disturbances. Determine a suitable sampling period $h$, and determine the poles (and characteristic polynomial) of the desired discrete-time closed-loop system.
Solution
Since we have dominating complex-conjugated poles, the sampling period should be such that
   $$ h\omega_n = 0.1 -- 0.6. $$
   Choose $$h = \frac{0.3}{\omega_n} = \frac{0.3}{\sqrt{5^2+5^2}} = \frac{0.3}{5\sqrt{2}} \approx 0.04. $$
   We get the discrete-time poles
   \begin{align}
   p_{d_1} &= \exp{0.04(-5+i5)} = \exp(-0.2) \exp(i0.2) = 0.81\big(\cos(0.21) + i\sin(0.21)\big) \approx 0.80 + i0.16\
   p_{d_2} &= p_{d_1}^ = 0.80 - i0.16\
   p_{d_3} &= \exp(ah) = \exp(-20\cdot 0.04) = 0.45.
   \end{align*}
Step2:  Design a 2-DoF controller
Assume a structure of the controller as given in figure \ref{fig
<ASSISTANT_TASK:>
Python Code:
import numpy as np
import sympy as sy
from sympy.utilities.codegen import codegen
import control.matlab as cm
import re
import matplotlib.pyplot as plt
from scipy import signal
z = sy.symbols('z', real=False)
r1,s0,s1 = sy.symbols('r1,s0,s1', real=True)
hh = sy.symbols('h', real=True, positive=True)
Bp = 2*(0.5*hh - 1 + sy.exp(-0.5*hh))*z + 2*(1-sy.exp(-0.5*hh) - 0.5*hh*sy.exp(-0.5*hh))
Ap = (z-sy.exp(-0.5*hh))*(z-1)
print(Bp)
print(Ap)
Explanation: RST design for a DC-motor
From HW3 2017 spring semester
Plant model
Zero-order-hold sampling of the DC-motor with transfer function
  $$ G(s) = \frac{1}{s(2s+1)} $$
  gives a discrete-time system with pulse transfer function 
  \begin{equation}
  G_d(z) = \frac{B(z)}{A(z)}= \frac{2\big(\frac{h}{2}-1+\mathrm{e}^{-\frac{h}{2}}\big)z + 2\big(1-\mathrm{e}^{-\frac{h}{2}}-\frac{h}{2}\mathrm{e}^{-\frac{h}{2}}\big)}{(z-1)\big(z-\mathrm{e}^{-\frac{h}{2}}\big)}.
  \end{equation}
End of explanation
Ap = sy.simplify(Ap.subs(hh, 0.04))
Bp = sy.simplify(Bp.subs(hh, 0.04))
print(Bp)
print(Ap)
Explanation: Determine sampling period and desired closed loop poles
In a continuous-time description of the desired closed-loop system we want the system to have two dominating poles at
   $$ -5 \pm i5. $$
   In addition to the two dominating poles, we want a third pole at
   $$ a=-20 $$
   to be able to control the response to disturbances. Determine a suitable sampling period $h$, and determine the poles (and characteristic polynomial) of the desired discrete-time closed-loop system.
Solution
Since we have dominating complex-conjugated poles, the sampling period should be such that
   $$ h\omega_n = 0.1 -- 0.6. $$
   Choose $$h = \frac{0.3}{\omega_n} = \frac{0.3}{\sqrt{5^2+5^2}} = \frac{0.3}{5\sqrt{2}} \approx 0.04. $$
   We get the discrete-time poles
   \begin{align}
   p_{d_1} &= \exp{0.04(-5+i5)} = \exp(-0.2) \exp(i0.2) = 0.81\big(\cos(0.21) + i\sin(0.21)\big) \approx 0.80 + i0.16\
   p_{d_2} &= p_{d_1}^ = 0.80 - i0.16\
   p_{d_3} &= \exp(ah) = \exp(-20\cdot 0.04) = 0.45.
   \end{align*}
End of explanation
z = sy.symbols('z', real=False)
r1,s0,s1,aa = sy.symbols('r1,s0,s1,aa', real=True)
Ac = z**2 - z*(0.8*2) + (0.8**2 + 0.16**2)
Ao = z-aa
Acl = Ac*Ao
Rp = z+r1
Sp = s0*z + s1
RHS = sy.simplify(sy.expand(Acl))
diopheq =sy.poly(Ap*Rp+Bp*Sp-Acl, z)
dioph = diopheq.all_coeffs()
print(sy.latex(sy.poly(RHS, z)))
print(dioph)
print(Acl)
print(Ap*Rp)
print(Ac)
print(Ap*Rp)
print(sy.poly(sy.simplify(sy.expand(Ap*Rp + Bp*Sp)), z))
sol = sy.solve(dioph, (r1,s0,s1))
print (-np.exp(-20*0.04) + 0.38)
print (- 0.98 + 0.6656)
print (1.6*np.exp(-20*0.04) - 0.98 + 0.6656)
print (-0.6656*np.exp(-20*0.04))
sol[r1].subs(aa, np.exp(-20*0.04))
print('r_1 = %f' % sol[r1].subs(aa, np.exp(-20*0.04)))
print('s_0 = %f' % sol[s0].subs(aa, np.exp(-20*0.04)))
print('s_1 = %f' % sol[s1].subs(aa, np.exp(-20*0.04)))
t0 = Ac.evalf(subs={z:1})/Bp.evalf(subs={z:1,})
print('t_0 = %f' % t0)
R = Rp.subs(sol)
S = Sp.subs(sol)
T = t0*Ao
Hc = T*Bp/(Ac*Ao)
Hcc = t0*0.8/Ac
sy.pretty_print(sy.expand(Hc))
sy.pretty_print(sy.expand(Hcc))
sy.pretty_print(Hc.evalf(subs={z:1}))
sy.pretty_print(sy.simplify(Ap*R + Bp*S))
0 + 2*1j
sy.Poly((Ac*Ao).subs(aa, -0.2), z).all_coeffs()
def my_bode(num, den, h=1.0):
    n = len(den)-1
    m = len(num)-1
    w = np.linspace(0.01, np.pi, 400);
    z = np.exp(1j*w);
    dd = den[-1]
    nd = num[-1]
    for i in range(n):
        dd += den[i]*np.power(z, n-i)
    for i in range(m):
        nd += num[i]*np.power(z, m-i)
    return nd/dd,w
def bodeplots(a, aa, Ap, R, Ac, Ao):
    numS = sy.list2numpy(sy.Poly((Ap*R).subs(aa, a), z).all_coeffs(), 
                        dtype=np.float64)
    numT = sy.list2numpy(sy.Poly((Bp*S).subs(aa, a), z).all_coeffs(), 
                        dtype=np.float64)
    den = sy.list2numpy(sy.Poly((Ac*Ao).subs(aa, a), z).all_coeffs(), 
                        dtype=np.float64)
    
    qS, wS = my_bode(numS, den)
    qT, wT = my_bode(numT, den)
    plt.figure()
    plt.loglog(wS, np.abs(qS))
    plt.loglog(wT, np.abs(qT))
    plt.ylim((0.001, 10))
    plt.legend(('Ss', 'Tt'))
    #cm.bode([Ss, Tt])
bodeplots(-0.009, aa, Ap, R, Ac, Ao)
from ipywidgets import interact, interactive, fixed, interact_manual
import ipywidgets as widgets
from functools import partial
interact(bodeplots, a=(0.0, 1.0), aa=fixed(aa), Ap=fixed(Ap), R=fixed(R), Ac=fixed(Ac),
        Ao=fixed(Ao))
# Reorganize solution expression for matlab code generation
sol_expr = ('RST_DC_lab', [Bp.all_coeffs()[0], Bp.all_coeffs()[1],
                           Ap.all_coeffs()[1], Ap.all_coeffs()[2],
                           sol[r1], sol[s0], sol[s1], A2p.subs(z, 1)/Bp.subs(z,1), h,np.exp(h*po1) ])  
# Export to matlab code
[(m_name, m_code)] = codegen(sol_expr, 'octave')
m_code = m_code.replace("out1", "b0").replace("out2", "b1").replace("out3", "a1").replace("out4", "a2")
m_code = m_code.replace("out5", "r1").replace("out6", "s0").replace("out7", "s1").replace("out8", "t0")
m_code = m_code.replace("out9", "h").replace("out10", "obsPole")
m_code = m_code.replace("function ", "% function ")
m_code = m_code.replace("end", "")
print m_code
with open("/home/kjartan/Dropbox/undervisning/tec/MR2007/labs/dc_rst_design.m", "w") as text_file:
    text_file.write(m_code)
cm.step?
G = Km * cm.tf([1], [tau, 1, 0])
Gd = Km * cm.tf([tau*(hpt-1+np.exp(-hpt)), tau*(1-(1+hpt)*np.exp(-hpt))], [1, -(1+np.exp(-hpt)), np.exp(-hpt)], h)
Gd2 = cm.c2d(G, h)
print Gd
print Gd2
print A2p
print A2p.evalf(subs={z:1})
print Bp
print Bp.evalf(subs={z:1})
0.3/(5*np.sqrt(2))
np.exp(-0.21)*np.sin(0.21)
np.exp(0.03*(-14))
 0.746*41.8
Explanation: Design a 2-DoF controller
Assume a structure of the controller as given in figure \ref{fig:2dof}. The controller is given by 
   $$ R(q)u = -S(q)y + T(q)u_c. $$
   With the plant-model
   $$ A(q)y = B(q)u$$
   we get the following difference equation for the closed-loop system
   $$ \big( A(q)R(q) + B(q)S(q) \big) y = B(q)T(q) u_c. $$
   Assume a suitable order (as low as possible) of the controller polynomials $R(q)$ and $S(q)$ and solve the diophantine equation 
   $$ A(q)R(q) + B(q)S(q)  = Ac(q) $$
   for $R$ and $S$. 
Solve the equations for arbitrary $a$: Use a symbol $a$ in your calculations so that you can easily recalculate your controller for a different value of $a$.
End of explanation
<END_TASK>