#!/usr/bin/python
# 9.11

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

import scipy as sp
from scipy import signal
from myc2d import myc2d

# Motor control problem
# Transfer function

a1 = sp.array([[-1, 0], [1, 0]]) 
b1 = sp.array([[1],[0]]) 
c1 = sp.array([0, 1])
d1 = 0
G = signal.lti(a1, b1, c1, d1)
Ts = 0.25
B, A, k = myc2d(G,Ts)

# Transient specifications
rise = 3
epsilon = 0.05
phi = desired(Ts,rise,epsilon)

# Controller design
Delta = 1 # No internal model of step used
Rc, Sc, Tc, gamm = pp_im(B, A, k, phi, Delta)

# simulation parameters
st = 1 # desired change in position
t_init = 0 # simulation start time
t_final = 10 # simulation end time

xInitial = [0, 0] # initial conditions
N = 1
C = 0
D = 1
N_var = 0