In [3]:

    

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
sy.init_printing()


    

In [16]:

    

z = sy.symbols('z', real=False)
r1,r2,s0,s1,s2 = sy.symbols('r1,r2,s0,s1,s2', real=True)
hh,a = sy.symbols('h,a', real=True, positive=True)


# The plant
Bp = sy.poly(1./6*(z**2 + 4*z + 1), z)
Ap = sy.poly( sy.expand((z-1)**3), z)

# The desired characteristic polynomial
aa = 0.1
Ac = sy.poly( sy.expand((z-0.8)**3), z)
Ao = sy.poly(sy.expand((z-a)**2), z)
Acl = Ac*Ao

Rp = sy.poly(z**2+r1*z+r2, z)
Sp = sy.poly(s0*z**2 + s1*z + s2, z)
dioph=(Ap*Rp + Bp*Sp - Acl).all_coeffs()
print dioph
print Acl
print Ap*Rp
print Ac
print Ap*Rp
print Ap*Rp + Bp*Sp


    

    




[2.0*a + 1.0*r1 + 0.166666666666667*s0 - 0.6, -1.0*a**2 - 4.8*a - 3.0*r1 + 1.0*r2 + 0.666666666666667*s0 + 0.166666666666667*s1 + 1.08, 2.4*a**2 + 3.84*a + 3.0*r1 - 3.0*r2 + 0.166666666666667*s0 + 0.666666666666667*s1 + 0.166666666666667*s2 - 0.488, -1.92*a**2 - 1.024*a - 1.0*r1 + 3.0*r2 + 0.166666666666667*s1 + 0.666666666666667*s2, 0.512*a**2 - 1.0*r2 + 0.166666666666667*s2]
Poly(1.0*z**5 + (-2.0*a - 2.4)*z**4 + (1.0*a**2 + 4.8*a + 1.92)*z**3 + (-2.4*a**2 - 3.84*a - 0.512)*z**2 + (1.92*a**2 + 1.024*a)*z - 0.512*a**2, z, domain='RR[a]')
Poly(z**5 + (r1 - 3)*z**4 + (-3*r1 + r2 + 3)*z**3 + (3*r1 - 3*r2 - 1)*z**2 + (-r1 + 3*r2)*z - r2, z, domain='ZZ[r1,r2]')
Poly(1.0*z**3 - 2.4*z**2 + 1.92*z - 0.512, z, domain='RR')
Poly(z**5 + (r1 - 3)*z**4 + (-3*r1 + r2 + 3)*z**3 + (3*r1 - 3*r2 - 1)*z**2 + (-r1 + 3*r2)*z - r2, z, domain='ZZ[r1,r2]')
Poly(1.0*z**5 + (1.0*r1 + 0.166666666666667*s0 - 3.0)*z**4 + (-3.0*r1 + 1.0*r2 + 0.666666666666667*s0 + 0.166666666666667*s1 + 3.0)*z**3 + (3.0*r1 - 3.0*r2 + 0.166666666666667*s0 + 0.666666666666667*s1 + 0.166666666666667*s2 - 1.0)*z**2 + (-1.0*r1 + 3.0*r2 + 0.166666666666667*s1 + 0.666666666666667*s2)*z - 1.0*r2 + 0.166666666666667*s2, z, domain='RR[r1,r2,s0,s1,s2]')

In [17]:

    

sol = sy.solve(dioph, (r1,r2,s0,s1,s2))
print 'r_1 = %f' % sol[r1].subs(a, aa)
print 'r_2 = %f' % sol[r2].subs(a, aa)
print 's_0 = %f' % sol[s0].subs(a, aa)
print 's_1 = %f' % sol[s1].subs(a, aa)
print 's_1 = %f' % sol[s1].subs(a, aa)


    

    




r_1 = 0.299367
r_2 = 0.089313
s_0 = 0.603800
s_1 = -1.102480
s_1 = -1.102480

In [18]:

    

# Check stability of the feedback controller
np.abs(np.roots([1, sol[r1].subs(a, aa), sol[r2].subs(a, aa) ]))


    

    
Out[18]:





array([ 0.29885336,  0.29885336])

In [19]:

    

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


    

    




t_0 = 0.008000

In [ ]:

    




    

In [8]:

    

1.0/0.3125


    

    
Out[8]:





3.2

In [53]:

    

num = sy.list2numpy((Ap*R).all_coeffs(), dtype=np.float64)
den = sy.list2numpy((Ac*Ao).all_coeffs(), dtype=np.float64)
print num
print den
print type(num[0])
#Hd = cm.tf(num[:-1], den[:-1], -1)
Hd = cm.tf([1], [1, 0.5])
print Hd
ystep, t = cm.step(Hd, np.arange(30))
plt.figure()
plt.plot(t, ystep)
plt.show()


    

    




[ 1.   -0.7  -0.08  0.  ]
[ 1.  -0.7  0.   0. ]
<type 'numpy.float64'>

   1
-------
s + 0.5

In [45]:

    

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


    

In [46]:

    

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


    

In [47]:

    

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)


    

    




% function [b0, b1, a1, a2, r1, s0, s1, t0, h, b00] = RST_DC_lab()
  %RST_DC_LAB  Autogenerated by sympy
  %   Code generated with sympy 0.7.6.1
  %
  %   See http://www.sympy.org/ for more information.
  %
  %   This file is part of 'project'

  b0 = 0.0905485642505141;
  b1 = 0.0860565404440641;
  a1 = -1.85841144421398;
  a2 = 0.858411444213975;
  r1 = -0.632898354797954;
  s0 = 0.37929607879435;
  s1 = -0.324459627611956;
  t0 = 0.37087688639506;
  h = 0.04;
  b00 = 0.852143788966211;

In [40]:

    

cm.step?


    

In [10]:

    

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


    

    




  0.09055 z + 0.08606
----------------------
z^2 - 1.858 z + 0.8584

dt = 0.04


  0.09055 z + 0.08606
----------------------
z^2 - 1.858 z + 0.8584

dt = 0.04

In [33]:

    

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


    

    




Poly(z**2 - 1.58555290309441*z - (-0.792776451547206 + 0.168974310731771*I)*(0.792776451547206 + 0.168974310731771*I), z, domain='EX')
0.0714939167206446
Poly(0.00133942860759726*z + 0.00124712240506047, z, domain='RR')
0.00258655101265772

In [4]:

    

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


    

    
Out[4]:





0.042426406871192847

In [6]:

    

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


    

    
Out[6]:





0.16897431073177133

In [7]:

    

np.exp(0.03*(-14))


    

    
Out[7]:





0.65704681981505675

In [42]:

    

0.746*41.8


    

    
Out[42]:





31.182799999999997

In [ ]: