# 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