#!/usr/bin/python
# 9.21

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

import scipy as sp
from myc2d import myc2d
from scipy import signal
from desired import desired
from pd import pd

# Motor control problem
# Transfer function

a = sp.array([[-1, 0], [1, 0]]) 
b = sp.array([[1],[0]]) 
c = sp.array([0, 1])
d = 0
G = signal.lti(a,b,c,d)
Ts = 0.25
B, A, k = myc2d(G,Ts)
num, den = G.num, G.den

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

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

# continuous time controller
K, taud, N = pd(Rc, Sc, Ts)
numb = K*sp.array([1, taud*(1+1/N)]) 
denb = sp.array([1, taud/N])
numf = 1
denf = 1

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

# continuous controller simulation: g_s_cl3.cos
num1 = 0
den1 = 1

# discrete controller simulation: g_s_cl2.cos
# u1: -0.1 to 0.8
# y1: 0 to 1.4
C = 0
D = 1
N = 1
gamm = 1
Tc = Sc