# Updated(2-8-07)
# 9.13 

import os, sys
sys.path += [os.getcwdu() + os.sep + ".." + os.sep + "python"]

import scipy as sp
from scipy import signal
from myc2d import myc2d
from dscr import dscr
from zpowk import zpowk
from desired import desired
from pp_im2 import pp_im2

num = 200
den = sp.convolve([0.05, 1], [0.05, 1])
den = sp.convolve([10, 1], den)
G = signal.lti(num,den)
Ts = 0.025
num = G.num 
den = G.den
B, A, k = myc2d(G, Ts)
zk, dzk = zpowk(k)

# Transient specifications
a = 0.9
rise = 0.24
epsilon = 0.05
phi = desired(Ts, rise, epsilon)

# Controller design
Delta = sp.array([1, -1]) # internal model of step is present
Rc, Sc, Tc, gamm = pp_im2(B, A, k, phi, Delta, a)

# margin calculation
Lnum = sp.convolve(Sc, sp.convolve(B,zk))
Lden = sp.convolve(Rc, A)
L = signal.lti(Lnum, Lden)
L = dscr(L, Ts)
Gm = g_margin(L)
Pm = p_margin(L)

num1 = 100
den1 = [10, 1]
Gd = signal.lti(num1, den1)
C, D, k1 = myc2d(Gd, Ts)
zk, dzk = zpowk(k)
C = sp.convolve(C, zk)

# simulation parameters g_s_cl2
N = 1
st = 1 # desired change in setpoint
st1 = 0 # magnitude of disturbance
t_init = 0 # simulation start time
t_final = 1.5 # simulation end time