# -*- 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.5

plant = massspringdamper.SingleMass(m,k,b)

#Controller
kp  = 1
ki  = 1
kd  = 1
tau = 0.1

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

pid.rbar[0] = 2 # set point


closed_loop_sys = pid + plant

closed_loop_sys.compute_trajectory(30)
closed_loop_sys.plot_trajectory('y')
closed_loop_sys.animate_simulation()