using ControlSystems

p = tf("s")

τ_1 = 3 # second
τ_2 = 0.1 # second

# Approximatively our system
H = 1 / p * 1 / (1 + τ_1 * p) * 1 / (1 + τ_2 * p)

# Speed corrector
Kp_speed = 0.5
Ki_speed = 0
Kd_speed = 0
C_speed = Kp_speed + Ki_speed * 1/p + Kd_speed * p

# Looped system with the speed controller
H_speed = C_speed * H / (1 + p * C_speed * H)


# Position corrector
Kp = 10
Ki = 0
Kd = 50

C = Kp + Ki * 1/p + Kd * p

# Looped transfer function
H_tot = H_speed * C / (1 + H_speed * C)

stepplot(H_tot)