#!/usr/bin/python 

# 9.9

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 pp_im import pp_im

from desired import desired

# Magnetically suspended ball problem

# Operating conditions

M = 0.05

L = 0.01

R = 1

K = 0.0001

g = 9.81

#Equilibrium conditions

hs = 0.01

i = sqrt(M*g*hs/K)

# State space matrices

a21 = K*i**2/M/hs**2

a23 = - 2*K*i/M/hs

a33 = - R/L

b3 = 1/L

a = sp.array([[0, 1, 0], [a21, 0, a23], [0, 0, a33]])

b = sp.array([[0], [0], [b3]])

c = sp.array([1, 0, 0])

d = 0

# Transfer functions

G = signal.lti(a, b, c, d)

Ts = 0.01

B, A, k = myc2d(G,Ts)

# Transient specifications

rise = 0.15

epsilon = 0.05

phi = desired(Ts, rise, epsilon)

# Controller design

Delta = sp.array([1, -1])

Rc, Sc, Tc, gamm = pp_im(B, A, k, phi)

# Setting up simulation parameters for basic

st = 0.0001 # desired change in h, in m.

t_init = 0  # simulation start time

t_final = 0.5 # simulation end time

# Setting up simulation parameters for c_ss_cl

N_var = 0

xInitial = [0 0 0]

N = 1

C = 0 

D = 1