import numpy as np
from sympy import *
from IPython.display import Math

PI = np.pi
K, zeta, omega = symbols('K zeta omega_n')  # actuator symbols
Ki, Kp, Kd, K2 = symbols('Ki Kp Kd K2')     # controller symbols
lam, s = symbols('lambda s')                # transfer function symbols
Gc = Ki/s + Kp + Kd*s + K2*s**2             # PID2 controller 
Ga = (K*omega**2)/(s*(s**2+2*zeta*omega*s+omega**2))    # actuator transfer function
display(Math('\nGc = '+latex(Gc)))              # the symbolic formula for the integrator gain
display(Math('\nGa = '+latex(Ga)))              # the symbolic formula for the integrator gain

$\displaystyle Gc = K_{2} s^{2} + Kd s + \frac{Ki}{s} + Kp$

$\displaystyle Ga = \frac{K \omega_{n}^{2}}{s \left(\omega_{n}^{2} + 2 \omega_{n} s \zeta + s^{2}\right)}$

cltf = simplify((Ki/s*Ga)/(1+Gc*Ga))            # closed loop transfer function
display(Math('\nCLTF = '+latex(cltf)))          # the symbolic formula for the closed loop transfer function

$\displaystyle CLTF = \frac{K Ki \omega_{n}^{2}}{K \omega_{n}^{2} \left(Ki + s \left(K_{2} s^{2} + Kd s + Kp\right)\right) + s^{2} \left(\omega_{n}^{2} + 2 \omega_{n} s \zeta + s^{2}\right)}$

num, den = fraction(simplify(cltf))             # the denominator is the characteristic equation
difce = simplify(expand(den-(s+lam)**4))        # difference between actual and desired CE
ki = solve(difce.coeff(s,0), Ki)[0]             # returns a list of one element
kp = solve(difce.coeff(s,1), Kp)[0]
kd = solve(difce.coeff(s,2), Kd)[0]
k2 = solve(difce.coeff(s,3), K2)[0]
display(Math('\nKi = '+latex(ki)))              # the symbolic formula for the integrator gain
display(Math('\nKd = '+latex(kp)))              # the symbolic formula for the proportional gain
display(Math('\nKd = '+latex(kd)))              # the symbolic formula for the derivative gain 
display(Math('\nKd = '+latex(k2)))              # the symbolic formula for the second derivative gain 

$\displaystyle Ki = \frac{\lambda^{4}}{K \omega_{n}^{2}}$

$\displaystyle Kd = \frac{4 \lambda^{3}}{K \omega_{n}^{2}}$

$\displaystyle Kd = \frac{6 \lambda^{2} - \omega_{n}^{2}}{K \omega_{n}^{2}}$

$\displaystyle Kd = \frac{2 \cdot \left(2 \lambda - \omega_{n} \zeta\right)}{K \omega_{n}^{2}}$

_difce = difce.subs([(K,10),(zeta,0.333333),(omega,2*PI*10),(lam,2*PI*10)])
_ki = solve(_difce.coeff(s,0), Ki)[0]          # returns a list of one element
_kp = solve(_difce.coeff(s,1), Kp)[0]
_kd = solve(_difce.coeff(s,2), Kd)[0]
_k2 = solve(_difce.coeff(s,3), K2)[0]
print(f'Ki={_ki:8.6f}, Kp={_kp:8.6f}, Kd={_kd:8.6f}, k2={_k2:8.6f}')

Ki=394.784176, Kp=25.132741, Kd=0.500000, k2=0.005305

_cltf = cltf.subs([(K,10),(zeta,0.333333),(omega,2*PI*10),(lam,2*PI*10), (Ki,_ki), (Kp,_kp), (Kd, _kd), (K2,_k2)])
_num, _den =  fraction(simplify(_cltf))         # the denominator is the characteristic equation

from bodeplot import bodeplot
from pzplot import pzplot
if _num.as_poly() == None:
    num_coeffs = [float(_num)]
else:
    num_coeffs = list(map(float,_num.as_poly().all_coeffs())) # convert list of symbols to list of floats
den_coeffs = list(map(float,_den.as_poly().all_coeffs())) # convert list of symbols to list of floats
zeros = np.roots(num_coeffs)
poles = np.roots(den_coeffs)
bodeplot(num_coeffs, den_coeffs)
pzplot(poles, zeros)

from const import BAR, DT, LPM, MM, KG, HZ, L, IN, M2MM
import hydsys                           # the main hydraulic system class to which other obects are added
import valve
import cylinder
import model
import moveabsolute
hydsys = hydsys.HydSys()
cyl0 = cylinder.Cylinder(hydsys)
cyl0.cyl_len = 1000*MM
cyl0.cyl_dia = 63*MM
cyl0.rod_dia = 28*MM
cyl0.mass = 1000*KG                     # load
model0 = model.Model(cyl0)              # add model to cylinder
model0.ext_gain = 10                    # extending open loop gain, (mm/s)/%
model0.ret_gain = 10
model0.damping_factor = 0.333333333333
model0.natural_freq =10*HZ
ma0 = moveabsolute.MoveAbsolute(cyl0)
vel, acc = ma0.calc_time_move(400, 1)
ma0.cmd_pos = 10                       # remove comment to override
ma0.cmd_vel = 100
ma0.cmd_acc = 1000
ma0.cmd_dec = 1000
ma0.mode = 2