#!/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