# -*- coding: utf-8 -*-
"""
Created on Jun 2 2021

@author: Alex
"""

import numpy as np

from pyro.dynamic import tranferfunction
from pyro.dynamic import statespace
from pyro.dynamic import massspringdamper
from pyro.control import linear


# Plant
m = 1
k = 1
b = 0

plant = massspringdamper.SingleMass(m,k,b)
tf    = tranferfunction.ss2tf( plant, 0, 0)

tf.bode_plot()
tf.pz_map()


#Controller
kp  = 5
ki  = 2
kd  = 4
tau = 0.1

pid = linear.PIDController(kp,ki,kd,tau)

pid.rbar[0] = 1 # set point


closed_loop_sys = pid + plant

cl_ss = statespace.linearize( closed_loop_sys )
cl_tf = tranferfunction.ss2tf( cl_ss, 0, 0)

cl_tf.bode_plot()
cl_tf.pz_map()

closed_loop_sys.plot_trajectory('y')
closed_loop_sys.plot_trajectory('u')
closed_loop_sys.animate_simulation()