# 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}


```python
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
```


```python
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)
```

    -1.0*h*exp(-0.5*h) + z*(h + 2*exp(-0.5*h) - 2) - 2*exp(-0.5*h) + 2
    (z - 1)*(z - exp(-0.5*h))


## 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*}



```python
Ap = sy.simplify(Ap.subs(hh, 0.04))
Bp = sy.simplify(Bp.subs(hh, 0.04))
print(Bp)
print(Ap)
```

    0.000397346613510541*z + 0.000394706454219282
    (z - 1)*(z - 0.980198673306755)


## 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$. 


```python
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))
```

    \operatorname{Poly}{\left( - 0.6656 aa + 1.0 z^{3} + z^{2} \left(- 1.0 aa - 1.6\right) + z \left(1.6 aa + 0.6656\right), z, domain=\mathbb{R}\left[aa\right] \right)}
    [1.0*aa + 1.0*r1 + 0.000397346613510541*s0 - 0.380198673306755, -1.6*aa - 1.98019867330676*r1 + 0.000394706454219282*s0 + 0.000397346613510541*s1 + 0.314598673306755, 0.6656*aa + 0.980198673306755*r1 + 0.000394706454219282*s1]
    (-aa + z)*(z**2 - 1.6*z + 0.6656)
    (r1 + z)*(z - 1)*(z - 0.980198673306755)
    z**2 - 1.6*z + 0.6656
    (r1 + z)*(z - 1)*(z - 0.980198673306755)
    Poly(1.0*z**3 + (1.0*r1 + 0.000397346613510541*s0 - 1.98019867330676)*z**2 + (-1.98019867330676*r1 + 0.000394706454219282*s0 + 0.000397346613510541*s1 + 0.980198673306755)*z + 0.980198673306755*r1 + 0.000394706454219282*s1, z, domain='RR[r1,s0,s1]')



```python
sol = sy.solve(dioph, (r1,s0,s1))
```


```python
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))

```

    -0.0693289641172
    -0.3144
    0.404526342588
    -0.299073358516



```python
sol[r1].subs(aa, np.exp(-20*0.04))
```




    -0.195457575341184




```python
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))
```

    r_1 = -0.195458
    s_0 = 317.927170
    s_1 = -272.319090
    t_0 = 82.822733
                                                                                  
                        - -0.0329093326044447⋅aa⋅z                                
    - ──────────────────────────────────────────────────────────────── - ─────────
            2                              3              2                    2  
      - aa⋅z  + 1.6⋅aa⋅z - - -0.6656⋅aa + z  - -1.6⋅(-1)⋅z  + 0.6656⋅z   - aa⋅z  +
    
                                                                                  
              - -0.0326906673955553⋅aa                                            
    ─────────────────────────────────────────────────────── + ────────────────────
                                3              2                    2             
     1.6⋅aa⋅z - - -0.6656⋅aa + z  - -1.6⋅(-1)⋅z  + 0.6656⋅z   - aa⋅z  + 1.6⋅aa⋅z -
    
                         2                                                        
     0.0329093326044447⋅z                                                0.0326906
    ──────────────────────────────────────────── + ───────────────────────────────
                     3              2                    2                        
     - -0.6656⋅aa + z  - -1.6⋅(-1)⋅z  + 0.6656⋅z   - aa⋅z  + 1.6⋅aa⋅z - - -0.6656⋅
    
                                     
    673955553⋅z                      
    ─────────────────────────────────
          3              2           
    aa + z  - -1.6⋅(-1)⋅z  + 0.6656⋅z
         66.2581866520861    
    ─────────────────────────
     2                       
    z  - -1.6⋅(-1)⋅z + 0.6656
    (--82.8227333151076⋅(-1)⋅aa + 82.8227333151076⋅z)⋅(0.000397346613510541⋅z + 0.
    ──────────────────────────────────────────────────────────────────────────────
                                            ⎛ 2                       ⎞           
                                  (-aa + z)⋅⎝z  - -1.6⋅(-1)⋅z + 0.6656⎠           
    
    000394706454219282)
    ───────────────────
                       
                       
                 2                                  3              2              
    - - -1.0⋅aa⋅z  + 1.6⋅aa⋅z - - -0.6656⋅aa + 1.0⋅z  - -1.6⋅(-1)⋅z  + 0.665600000
    
            
    000001⋅z



```python
0 + 2*1j
```




    2j




```python
sy.Poly((Ac*Ao).subs(aa, -0.2), z).all_coeffs()
```




    [1.00000000000000, -1.40000000000000, 0.345600000000000, 0.133120000000000]




```python
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)

```


```python
from ipywidgets import interact, interactive, fixed, interact_manual
import ipywidgets as widgets
from functools import partial
```


```python
interact(bodeplots, a=(0.0, 1.0), aa=fixed(aa), Ap=fixed(Ap), R=fixed(R), Ac=fixed(Ac),
        Ao=fixed(Ao))
```


    interactive(children=(FloatSlider(value=0.5, description='a', max=1.0), Output()), _dom_classes=('widget-inter…





    <function __main__.bodeplots(a, aa, Ap, R, Ac, Ao)>




```python
# 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) ])  
```


```python
# Export to matlab code
[(m_name, m_code)] = codegen(sol_expr, 'octave')
```


```python
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)
```


```python
cm.step?
```


```python

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
```


```python
print A2p
print A2p.evalf(subs={z:1})
print Bp
print Bp.evalf(subs={z:1})

```


```python
0.3/(5*np.sqrt(2))
```


```python
np.exp(-0.21)*np.sin(0.21)
```


```python
np.exp(0.03*(-14))
```


```python
 0.746*41.8

```


```python

```

728cbf6c0e41c6ed75c999cf2f2cbf289ebeb946
ipynb
Jupyter Notebook
polynomial-design/notebooks/.ipynb_checkpoints/RST-DC-motor-single-observer-pole-checkpoint.ipynb
kjartan-at-tec/mr2007-computerized-control
16e35f5007f53870eaf344eea1165507505ab4aa
MIT
2020-11-07T05:20:37.000Z
2020-12-22T09:46:13.000Z
polynomial-design/notebooks/.ipynb_checkpoints/RST-DC-motor-single-observer-pole-checkpoint.ipynb
kjartan-at-tec/mr2007-computerized-control
16e35f5007f53870eaf344eea1165507505ab4aa
MIT
2020-06-12T20:44:41.000Z
2020-06-12T20:49:00.000Z
polynomial-design/notebooks/.ipynb_checkpoints/RST-DC-motor-single-observer-pole-checkpoint.ipynb
kjartan-at-tec/mr2007-computerized-control
16e35f5007f53870eaf344eea1165507505ab4aa
MIT
2021-03-14T03:55:27.000Z
2021-03-14T03:55:27.000Z
Qwen/Qwen-72B
1. YES
2. YES