In [1]:
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
In [2]:
z = sy.symbols('z', real=False)
# The plant model
wh = np.pi/6;
cwh = np.cos(wh);
b1 = 1-cwh;
b2 = b1;
a1 = -2*cwh;
a2 = 1;
Bp = sy.poly(b1*z + b2, z)
Ap = sy.poly( z**2 + a1*z + a2, z)
# Poles and zeros
H = cm.tf([0, b1, b2], [1, a1, a2], 1)
pz = cm.pzmap(H)
np.abs(pz)
Out[2]:
In [3]:
p1 = 0.6
p2 = p1
Ac = sy.poly((z-p1)*(z-p2), z)
Ao = sy.poly(z, z)
In [4]:
r1,s0,s1, s2 = sy.symbols('r1,s0,s1, s2', real=True)
# Right hand side
Acl = Ac*Ao
# Left hand side
Rp = sy.poly(z+r1, z)
Sp = sy.poly(s0*z + s1, 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)
In [5]:
sol = sy.solve(dioph, (r1,s0,s1))
print('r_1 = %f' % sol[r1])
print('s_0 = %f' % sol[s0])
print('s_1 = %f' % sol[s1])
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))
In [6]:
1.0/0.3125
Out[6]:
In [7]:
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)
print(Hd)
ystep, t = cm.step(Hd)
plt.figure()
plt.plot(t, ystep)
plt.show()
In [10]:
# The observer poles: Should be twice as fast as the closed-loop poles
# and critically damped
p1 = 0.6 + 1j*0.3;
p2 = 0.6 - 1j*0.3;
lambda1 = np.log(p1)
omegan = np.abs(lambda1)
zeta = np.real(lambda1) / omegan
po1 = np.exp(-2*omegan)
print(lambda1, omegan, zeta, po1)
In [11]:
r1,s0,s1,s2 = sy.symbols('r1,s0,s1,s2', real=True)
# Right hand side
Ac = sy.poly((z-p1)*(z-p2), z)
#Ao = sy.poly(z**2, z)
Ao = sy.poly((z-po1)**2, z)
Acl = Ac*Ao
# Left hand side
Rp = sy.poly((z-1)*(z+r1), 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)
In [13]:
sol = sy.solve(dioph, (r1,s0,s1, s2))
sol
Out[13]:
In [14]:
print('r_1 = %f' % sol[r1])
print('s_0 = %f' % sol[s0])
print('s_1 = %f' % sol[s1])
print('s_2 = %f' % sol[s2])
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))
In [ ]:
# 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 [ ]:
# Export to matlab code
[(m_name, m_code)] = codegen(sol_expr, 'octave')
In [ ]:
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)
In [ ]:
cm.step?
In [ ]:
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
In [ ]:
print A2p
print A2p.evalf(subs={z:1})
print Bp
print Bp.evalf(subs={z:1})
In [ ]:
0.3/(5*np.sqrt(2))
In [ ]:
np.exp(-0.21)*np.sin(0.21)
In [ ]:
np.exp(0.03*(-14))
In [ ]:
0.746*41.8
In [ ]: