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

@author: Alex
"""
from pyro.dynamic               import massspringdamper
from pyro.control.linear        import PIDController


# Plant
sys = massspringdamper.FloatingTwoMass( output_mass = 1 )

sys.m1 = 1
sys.m2 = 1
sys.k2 = 3
sys.b2 = 1

sys.compute_ABCD()


# LQR controller
kp  = 2
ki  = 0
kd  = 0
tau = 0.01

ctl = PIDController(kp, ki, kd, tau)

ctl.rbar[0] = 2 # reference

# Simulation Closed-Loop Non-linear with LQR controller
cl_sys = ctl + sys

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