def margin_zpk(sys, z, p, k):
# open-loop system transfer function
try:
num, den = model(sys)
except:
# for error detection
print("Err: system in not defined")
return
Gs = control.tf(num, den)
# define compensator transfer function
# convert zero and pole to numpy arrays
z = np.array([z])
p = np.array([p])
num, den = matlab.zpk2tf(z, p, k)
Ds = matlab.tf(num, den)
# Compensated open-loop transfer function
DsGs = Ds*Gs
# phase & gain margins
gm, pm, wg, wp = control.margin(DsGs)
# round phase & gain margin variables
ndigits = 2
gm = round(gm, ndigits)
pm = round(pm, ndigits)
wg = round(wg, ndigits)
wp = round(wp, ndigits)
return gm, pm, wg, wp
def zpk(z, p, k):
Gs = control.tf(36, [1, 3.6, 0])
# Define Compensator transfer function
num, den = matlab.zpk2tf(z, p, k)
Ds = matlab.tf(num, den)
print(Ds)
# Draw the open-loop frequency resp. for the comp. sys
DsGs = Ds*Gs
gm, pm, wg, wp = control.margin(DsGs)
print(f"Gain margin = {gm} dB")
print(f"Phase margin = {round(pm, 2)} degrees")
print(f"Frequency for Gain Margin = {wg} radians/sec")
print(f"Frequecny for Phase Margin = {wp} radians/sec")
omega_comp = np.logspace(-2,2,2000)
mag_comp, phase_comp, omega_comp = control.bode(DsGs, omega=omega_comp)
mag_comp = 20 * np.log10(mag_comp)
phase_comp = np.degrees(phase_comp)
omega_comp = omega_comp.T
phase_comp = phase_comp.T
mag_comp = mag_comp.T
omega_comp = list(omega_comp)
phase_comp = list(phase_comp)
mag_comp = list(mag_comp)
return omega_comp, mag_comp, phase_comp, gm, pm, wp, wg
def action_zpk(sys, final_time, setpoint, z, p, k):
# open-loop system transfer function
try:
num, den = model(sys)
except:
# for error detection
print("Err: system in not defined")
return
Gs = control.tf(num, den)
z = np.array([z])
p = np.array([p])
num, den = matlab.zpk2tf(z, p, k)
Ds = matlab.tf(num, den)
# closed-loop unity-feedback transfer function
Ts = control.feedback(Ds*Gs, 1)
# simulation time parameters
initial_time = 0
nsteps = 40 * int(final_time) # number of time steps
t = np.linspace(initial_time, final_time, round(nsteps))
output, t = matlab.step(Ts, t)
output = setpoint*output
# calculate list of error
setpoint_arr = setpoint * np.ones(nsteps)
err = setpoint_arr - output
# calculate control action
action = matlab.lsim(Ds, err, t)
# covert numpy arrays to lists
t = list(t)
action = list(action[0])
# round lists to 6 decimal digits
ndigits = 6
t = [round(num, ndigits) for num in t]
action = [round(num, ndigits) for num in action]
# calculate maximum control action
max_action = max(action)
min_action = min(action)
return t, action, min_action, max_action
def step_zpk(sys, final_time, setpoint, z, p, k):
# open-loop system transfer function
try:
num, den = model(sys)
except:
# for error detection
print("Err: system in not defined")
return
Gs = control.tf(num, den)
# define compensator transfer function
# convert zero and pole to numpy arrays
z = np.array([z])
p = np.array([p])
num, den = matlab.zpk2tf(z, p, k)
Ds = matlab.tf(num, den)
# Compensated open-loop transfer function
DsGs = Ds*Gs
# closed-loop unity-feedback transfer function
Ts = control.feedback(DsGs, 1)
# simulation time parameters
initial_time = 0
nsteps = 40 * final_time # number of time steps
t = np.linspace(initial_time, final_time, round(nsteps))
output, t = matlab.step(Ts, t)
output = setpoint*output
# covert numpy arrays to lists
t = list(t)
output = list(output)
# round lists to 6 decimal digits
ndigits = 6
t = [round(num, ndigits) for num in t]
output = [round(num, ndigits) for num in output]
return t, output
def bode_zpk(sys, z, p, k):
# open-loop system transfer function
try:
num, den = model(sys)
except:
# for error detection
print("Err: system in not defined")
return
Gs = control.tf(num, den)
# define compensator transfer function
# convert zero and pole to numpy arrays
z = np.array([z])
p = np.array([p])
num, den = matlab.zpk2tf(z, p, k)
Ds = matlab.tf(num, den)
# Compensated open-loop transfer function
DsGs = Ds*Gs
# bode plot arrays
omega = np.logspace(-2, 2, 2000)
mag, phase, omega = control.bode(DsGs, omega=omega)
mag = 20 * np.log10(mag) # mag in db
phase = np.degrees(phase) # phase in degrees
# convert numpy arrays to lists
omega = list(omega)
phase = list(phase)
mag = list(mag)
# round lists to 6 decimal floating digits
ndigits = 6
omega = [round(num, ndigits) for num in omega]
phase = [round(num, ndigits) for num in phase]
mag = [round(num, ndigits) for num in mag]
return omega, mag, phase
from control import matlab
if True:
Ps = matlab.tf(*matlab.zpk2tf([
-0.01960354 - 0.97998085j, -0.01960354 + 0.97998085j, -0.01250354 -
2.50067649j, -0.01250354 + 2.50067649j, -0.02158274 -
4.31649435j, -0.02158274 + 4.31649435j, -0.06620907 -
5.29631139j, -0.06620907 + 5.29631139j, -0.10000737 -
5.99960852j, -0.10000737 + 5.99960852j, -0.03990608 -
6.3848482j, -0.03990608 + 6.3848482j, -0.15808494 -
7.90266611j, -0.15808494 + 7.90266611j, -0.14137167 -
8.48112199j, -0.14137167 + 8.48112199j, -3.38325363 -
43.85197895j, -3.38325363 + 43.85197895j
], [
-0.03023783 - 0.48285941j, -0.03023783 + 0.48285941j, -0.02739469 -
1.36946042j, -0.02739469 + 1.36946042j, -0.01747511 -
2.79596285j, -0.01747511 + 2.79596285j, -0.02280796 -
4.56153551j, -0.02280796 + 4.56153551j, -0.05943893 -
5.34917374j, -0.05943893 + 5.34917374j, -0.10136872 -
6.08127858j, -0.10136872 + 6.08127858j, -0.04618141 -
6.46523275j, -0.04618141 + 6.46523275j, -0.21441370 -
8.57386735j, -0.21441370 + 8.57386735j, -0.19949113 -
7.97715131j, -0.19949113 + 7.97715131j
], 3.64e-4))
Pa = Ps #*700
integ = matlab.tf([1], [1, 0])
from gwpy.frequencyseries import FrequencySeries
from control import matlab
from miyopy.utils.trillium import selfnoise
tr120 = matlab.tf(*matlab.zpk2tf([0, 0, -31.63, -160.0, -350.0, -3177.0], [
-0.036614 + 0.037059j,
-0.036614 - 0.037059j,
-32.55,
-142.0,
-364.0 + 404.0j,
-364.0 - 404.0j,
-1260.0,
-4900.0 + 5200.0j,
-4900.0 - 5200.0j,
-7100.0 + 1700.0j,
-7100.0 - 1700.0j,
], 1202.5 * 8.31871e17))
tr120_u = matlab.tf(*matlab.zpk2tf([0, 0, -31.63, -160.0, -350.0, -3177.0], [
-0.036614 + 0.037059j,
-0.036614 - 0.037059j,
-32.55,
-142.0,
-364.0 + 404.0j,
-364.0 - 404.0j,
-1260.0,
-4900.0 + 5200.0j,
-4900.0 - 5200.0j,
-7100.0 + 1700.0j,
-7100.0 - 1700.0j,
], 8.31871e17))
def step_zpk_servo(final_time, setpoint, z, p, k):
# Define Servomotor by Differential Equation
# 36/(s^2 + 3.6s)
def servo(y, t, u, extra):
extra = 0.0
dy_dt = y[1]
dy2_dt = 36*u - 3.6* y[1]
return [dy_dt, dy2_dt]
# simulation time parameters
nsteps = int(final_time*40) # number of time steps
t = np.linspace(0,final_time,nsteps) # linearly spaced time vector
delta_t = t[1] # how long each step is
# simulate step input operation
y0 = [0.0, 0.0] # servo initial degree
step = np.zeros(nsteps)
sp = setpoint # set point
step[0] = 0.0
step[1:] = sp
yt = np.zeros(nsteps) # for storing degrees results - degrees vector
yt[0] = y0[1]
et = np.zeros(1) # error value
#ut = np.zeros(nsteps) # actuation vector
# Controller TF
z = np.array([z])
p = np.array([p])
num, den = matlab.zpk2tf(z, p, k)
Ds = matlab.tf(num, den)
# Actuation limits
max_ac = 5
min_ac = -5
# simulate with ODEINT
for i in range(nsteps):
error = step[i] - y0[0]
et[0] = error
q = matlab.lsim(Ds, [0, et[0]], [0, t[i]])
q = list(q[0])
u = q[1]
# clip input to -100% to 100%
if u > max_ac:
u = max_ac
elif u < min_ac:
u = min_ac
# ut[i+1] = u
y = odeint(servo, y0, [0,delta_t],args=(u,0))
y0 = y[-1]
yt[i] = y0[0]
t = list(t)
output = list(yt)
# round lists to 4 decimal digits
ndigits = 4
t = [round(num, ndigits) for num in t]
output = [round(num, ndigits) for num in output]
return t, output
def step_zpk_cruise(final_time, setpoint, z, p, k):
# Define engine lag
def lag(p, t, u, extra):
# inputs
# u = power demand
# p = output power
# t = time
extra = 0.0
dp_dt = 2*(u - p)
return dp_dt
# Define car plant
def vehicle(v, t, p, load):
# inputs
# v = behicle speed
# t = time
# p = power
# load = cargo + passenger load
# Car specifications and initialization data
rho = 1.225 # kg/m3
CdA = 0.55 # m2
m = 1800 # kg
# Calculate derivative of vehicle velocity
dv_dt = (1.0/m) * (p*hp2watt/v - 0.5*CdA*rho*v**2)
return dv_dt
# Conversion factors
hp2watt = 746
mps2kmph = 3.6
kmph2mps = 1/mps2kmph
# simulation time parameters
tf = final_time # final time for simulation
nsteps = int(40* final_time) # number of time steps
t = np.linspace(0,tf,nsteps) # linearly spaced time vector
delta_t = t[1] # how long each step is
v0_kmph = 1.0 # car initial speed in km/h
sp_kmph = setpoint # car final speed in km/h
# simulate step input operation
load = 0.0 # passenger(s) and cargo load - in Kg
v0 = v0_kmph*kmph2mps # car initial speed
step = np.zeros(nsteps)
sp = sp_kmph*kmph2mps
step[0] = v0
step[1:] = sp
vt = np.zeros(nsteps) # for storing speed results - speed vector
vt[0] = v0
et = np.zeros(1) # error value
#ut = np.zeros(nsteps) # actuation vector
#pt = np.zeros(nsteps) # power vector
p0 = 0.0 # initial car power
# Actuation limits - Car Physical limits
max_power = 120
min_power = -120
# Controller TF
z = np.array([z])
p = np.array([p])
num, den = matlab.zpk2tf(z, p, k)
Ds = matlab.tf(num, den)
# simulate with ODEINT
for i in range(nsteps):
error = step[i] - v0
et[0] = error
q = matlab.lsim(Ds, [0, et[0]], [0, t[i]])
q = list(q[0])
u = q[1]
# clip input to -100% to 100%
if u > max_power:
u = max_power
elif u < min_power:
u = min_power
# ut[i+1] = u
p = odeint(lag, p0, [0,delta_t],args=(u,0))
v = odeint(vehicle,v0,[0,delta_t],args=(p[-1], load))
p0 = p[-1]
v0 = v[-1]
vt[i] = v0
# pt[i+1] = p0
# convert output to km/h
vt_kmph = vt*mps2kmph
t = list(t)
output = list(vt_kmph)
# round lists to 4 decimal digits
ndigits = 4
t = [round(num, ndigits) for num in t]
output = [round(num, ndigits) for num in output]
return t, output