calc_motor1()

syms s
%syms Kd Tya Km Ke Jsum i
W1 = Kd / (Tya * s + 1) * Km * 1/Jsum * 1/s
W2 = Ke
W3 = W1 / (1 + W1 * W2)
W = Un * W3 * 1/i * 1/s

W = simplify(W)
[B,A] = numden(W)
b = sym2poly(B)
a = sym2poly(A)
b = b/a(1)
a = a/a(1)
order = size(a)
order = order(2)

if order == 2
    w0 = 2.9 / Tp
    b0 = sym2poly(s^2 + 1.4 * w0 * s + w0^2)
else if order == 3
    w0 = 6.0 / Tp
    b0 = sym2poly(s^3 + 2 * w0 * s^2 + 2 * w0^2 * s + w0^3) % 3
end

A = zeros(order-1, 1)
A = [A eye(order-1)]
A = [A; -a]