def Ftrminmax(lK, Rg, alpha, betas, beta, delta, omegat, pos):
a = np.amin(Ftrxsum(lK, Rg, alpha, betas, beta, delta, omegat, pos))
b = np.amax(Ftrxsum(lK, Rg, alpha, betas, beta, delta, omegat, pos))
c = np.amin(Ftrysum(lK, Rg, alpha, betas, beta, delta, omegat, pos))
d = np.amax(Ftrysum(lK, Rg, alpha, betas, beta, delta, omegat, pos))
# print np.abs(a+b)+np.abs(c+d)
return (np.abs(a + b) + np.abs(c + d))
def evaluate_design(self, x_denorm):
with open('_eval/ACMConfig.h', 'r') as f:
new_line = []
for line in f.readlines():
if '#define CURRENT_LOOP_BANDWIDTH' in line:
new_line.append(
f'#define CURRENT_LOOP_BANDWIDTH (2*M_PI*{x_denorm[0]:g})\n'
) # 15
elif '#define DELTA_THE_DAMPING_FACTOR' in line:
new_line.append(
f'#define DELTA_THE_DAMPING_FACTOR ({x_denorm[1]:g})\n'
) # 0.08
else:
new_line.append(line)
with open('_eval/ACMConfig.h', 'w') as f:
f.writelines(new_line)
if os.path.exists(self.work_dir + '/_eval/main.exe'):
os.remove(self.work_dir + '/_eval/main.exe')
subprocess.run([
"gcc", "_eval/main.c", "_eval/controller.c", "_eval/observer.c",
"-o", "_eval/main"
])
while not os.path.exists(self.work_dir + '/_eval/main.exe'):
time.sleep(0.1)
print('sleep for main.exe')
# subprocess.run( [self.work_dir+'/_eval/main.exe'] )
os.system('cd _eval && main')
while not os.path.exists(self.work_dir + '/_eval/pi_opti.dat'):
time.sleep(0.1)
print('sleep for .dat')
speed_profile = np.loadtxt('_eval/pi_opti.dat', skiprows=1)
os.remove(self.work_dir + '/_eval/pi_opti.dat')
TS = 0.000025 # ACMConfig.h
print(max(speed_profile), 'rpm')
print(min(speed_profile, key=lambda x: abs(x - 90)), 'rpm') # [rpm]
print(np.abs(speed_profile - 90).argmin() * TS * 1000, 'ms') # [ms]
rise_time = np.abs(speed_profile - 90).argmin() * TS * 1000
over_shoot = max(speed_profile)
if over_shoot <= 100:
over_shoot = 100
# modify this function to call matlab and eMach to evaluate the design with free variables as in x_denorm
return over_shoot, rise_time
def plot_weightings():
"""Plots all weighting functions defined in :module: splweighting."""
from scipy.signal import freqz
from pylab import plt, np
sample_rate = 48000
num_samples = 2 * 4096
fig, ax = plt.subplots()
for name, weight_design in sorted(_weighting_coeff_design_funsd.items()):
b, a = weight_design(sample_rate)
w, H = freqz(b, a, worN=num_samples)
freq = w * sample_rate / (2 * np.pi)
ax.semilogx(freq,
20 * np.log10(np.abs(H) + 1e-20),
label='{}-Weighting'.format(name))
plt.legend(loc='lower right')
plt.xlabel('Frequency / Hz')
plt.ylabel('Damping / dB')
plt.grid(True)
plt.axis([10, 20000, -80, 5])
return fig, ax
def plot_weightings():
"""Plots all weighting functions defined in :module: splweighting."""
from scipy.signal import freqz
from pylab import plt, np
sample_rate = 48000
num_samples = 2*4096
fig, ax = plt.subplots()
for name, weight_design in sorted(
_weighting_coeff_design_funsd.items()):
b, a = weight_design(sample_rate)
w, H = freqz(b, a, worN=num_samples)
freq = w*sample_rate / (2*np.pi)
ax.semilogx(freq, 20*np.log10(np.abs(H)+1e-20),
label='{}-Weighting'.format(name))
plt.legend(loc='lower right')
plt.xlabel('Frequency / Hz')
plt.ylabel('Damping / dB')
plt.grid(True)
plt.axis([10, 20000, -80, 5])
return fig, ax
def c2v_design(R,
L,
n_pp,
J_s,
KE,
B=0,
CLBW_Hz=1000,
CL_TS=1 / 20e3,
fignum=5):
currentKp, currentKi = get_coeffs_dc_motor_current_regulator(R, L, CLBW_Hz)
currentKiCode = currentKi * currentKp * CL_TS
if True:
# 这里打印的用于实验中CCS的debug窗口检查电流环PI系数
上位机电流KP = CLBW_Hz
上位机电流KI = 1000
iSMC_currentKp = 上位机电流KP * L * 2 * np.pi
iSMC_currentKi = 上位机电流KI / 1000 * R / L
iSMC_currentKiCode = iSMC_currentKi * CL_TS * iSMC_currentKp
print(f'\tiSMC_currentKp={iSMC_currentKp:g}, \
iSMC_currentKi={iSMC_currentKi:g}, \
iSMC_currentKiCode={iSMC_currentKiCode:g}')
print(f'\tSimC_currentKp={currentKp:g}, \
SimC_currentKi={currentKi:g}, \
SimC_currentKiCode={currentKiCode:g}')
print(f'\t上位机电流KP={上位机电流KP:g}, \
上位机电流KI={上位机电流KI:g}')
Gi_closed = control.tf(
[1], [L / currentKp, 1]) # current loop zero-pole cancelled already
currentBandwidth_radPerSec = currentKp / L
KT = 1.5 * n_pp * KE
dc_motor_motion = control.tf([KT],
[J_s / n_pp, B]) # [Apk] to [elec.rad/s]
print(dc_motor_motion)
# quit()
# 注意，我们研究的开环传递函数是以电流给定为输入的，而不是以转速控制误差为输入，这样仿真和实验容易实现一点。
# Gw_open = dc_motor_motion * Gi_closed * speedPI
c2v_tf = dc_motor_motion * Gi_closed
# fig5 = plt.figure(fignum)
# plt.title('Designed Current Ref. to Velocity Meas. Transfer Function')
mag, phase, omega = control.bode_plot(c2v_tf,
2 * np.pi * np.logspace(0, 4, 500),
dB=1,
Hz=1,
deg=1,
lw='0.5',
label=f'{CLBW_Hz:g} Hz')
open_cutoff_frequency_HZ = omega[(np.abs(mag - 0.0)).argmin()] / 2 / np.pi
# print('\tCut-off frequency (without speed PI regulator):', open_cutoff_frequency_HZ, 'Hz')
return (currentKp, currentKi), \
(上位机电流KP, 上位机电流KI), \
(mag, phase, omega)
def find_nearest(array, value):
array = np.asarray(array)
idx = (np.abs(array - value)).argmin()
return idx, array[idx]
def reflection1(xvals, parms):
#position=p[0]; intensity=p[1]; slit=p[2]; stth=p[3]; background=p[4]; thickness=p[5]; absorption=p[6]
x0 = parms[0]
i0 = parms[1]
w = parms[2]
theta = parms[3]
ib = parms[4]
thick = parms[5]
u = parms[6]
th = np.deg2rad(theta)
p = w * np.sin(th)
Atot = w * w / np.sin(2.0 * th)
out = []
ni = 5
irng = np.array(range(1, ni + 1))
for x in xvals:
if x < (x0 - p):
val = ib
elif ((x0 - p) < x and x0 > x):
l1 = x - (x0 - p)
nrleft = int(np.ceil(l1 / (2 * p) * ni))
if nrleft < 1: nrleft = 1
irngleft = np.array(range(1, nrleft + 1))
dl1 = l1 / float(nrleft)
dl = irngleft * dl1
triA = dl * dl / np.tan(th) #triangle areas
secA = [triA[0]
] + [triA[i] - triA[i - 1]
for i in range(1, nrleft)] #section areas
secA = np.array(secA)
m1 = np.linspace(
x0 - p + dl1 / 2.0, x - dl1 / 2.0, nrleft
) #section midpoint position - path length calculated from this
plen = np.abs(2 * m1 / np.sin(th))
val = ib + np.sum(i0 * secA * np.exp(-u * plen))
elif (x0 <= x) and (x0 + p >= x):
l1 = p
nrleft = int(np.ceil(l1 / (2 * p) * ni))
if nrleft < 1: nrleft = 1
irngleft = np.array(range(1, nrleft + 1))
dl1left = l1 / float(nrleft)
dlleft = irngleft * dl1left
triAleft = dlleft * dlleft / np.tan(th) #triangle areas
secAleft = [triAleft[0]] + [
triAleft[i] - triAleft[i - 1] for i in range(1, nrleft)
] #section areas
secAleft = np.array(secAleft)
m1left = np.linspace(x0 - p + dl1left / 2.0, x0 - dl1left / 2.0,
nrleft) + (x - x0)
plenleft = np.abs(2 * m1left / np.sin(th))
valleft = np.sum(i0 * secAleft * np.exp(-u * plenleft))
l2 = x - x0
nrright = int(np.ceil(x - x0 / (2 * p) * ni))
if nrright < 1: nrright = 1
irngright = np.array(range(1, nrright + 1))
dl1right = l2 / float(nrright)
dlright = p - np.append(0.0, dl1right * irngright)
triAright = dlright * dlright / np.tan(th)
secAright = [
triAright[i] - triAright[i + 1] for i in range(nrright)
]
secAright = np.array(secAright)
m1right = np.linspace(x - x0 - dl1right / 2.0, dl1right / 2.0,
nrright)
plenright = np.abs(2 * m1right / np.sin(th))
valright = np.sum(i0 * secAright * np.exp(-u * plenright))
val = ib + valleft + valright
elif (x > x0 + p):
l1 = p
#nrleft = int(np.ceil(l1/(x-(x0-p))*ni));
nrleft = int(np.ceil(l1 / (2 * p) * ni))
if nrleft < 1: nrleft = 1
irngleft = np.array(range(1, nrleft + 1))
dl1left = l1 / float(nrleft)
dlleft = irngleft * dl1left
triAleft = dlleft * dlleft / np.tan(th) #triangle areas
secAleft = [triAleft[0]] + [
triAleft[i] - triAleft[i - 1] for i in range(1, nrleft)
] #section areas
secAleft = np.array(secAleft)
m1left = np.linspace(x0 - p + dl1left / 2.0, x0 - dl1left / 2.0,
nrleft) + (x - x0)
plenleft = np.abs(2 * m1left / np.sin(th))
valleft = np.sum(i0 * secAleft * np.exp(-u * plenleft))
l2 = p
nrright = int(np.ceil(l2 / (2 * p) * ni))
if nrright < 1: nrright = 1
irngright = np.array(range(1, nrright + 1))
dl1right = l2 / float(nrright)
dlright = p - np.append(0.0, dl1right * irngright)
triAright = dlright * dlright / np.tan(th)
secAright = [
triAright[i] - triAright[i + 1] for i in range(nrright)
]
secAright = np.array(secAright)
m1right = np.linspace(dlright[0] - dl1right / 2.0, dlright[-1] +
dl1right / 2.0, nrright) + (x - (x0 + p))
plenright = np.abs(2 * m1right / np.sin(th))
valright = np.sum(i0 * secAright * np.exp(-u * plenright))
val = ib + valleft + valright
out = out + [val]
return np.array(out)
def iterate_for_desired_bandwidth(delta,
desired_VLBW_Hz,
motor_dict,
CLBW_Hz_initial=1000,
CLBW_Hz_stepSize=100):
# print('DEBUG tuner.py', motor_dict)
R = motor_dict['Rs']
L = motor_dict['Ls']
J_s = motor_dict['J_s']
JLoadRatio = motor_dict['JLoadRatio']
n_pp = motor_dict['n_pp']
KE = motor_dict['KE']
CL_TS = motor_dict['CL_TS']
VL_TS = motor_dict['VL_TS']
J_total = J_s * (1 + JLoadRatio)
CLBW_Hz = CLBW_Hz_initial #100 # Hz (initial)
VLBW_Hz = -10 # Hz (initial)
count = 0
while True:
count += 1
if count > 20:
msg = f'Loop count 20 is reached. Step size is {CLBW_Hz_stepSize} Hz.'
print(msg)
# raise Exception()
break
# Current loop (Tune its bandwidth to support required speed response)
if abs(VLBW_Hz - desired_VLBW_Hz) <= 10: # Hz
break
else:
if VLBW_Hz > desired_VLBW_Hz:
CLBW_Hz -= CLBW_Hz_stepSize # Hz
if CLBW_Hz <= 0:
raise Exception(
f'Negative CLBW_Hz. Maybe change the step size of "CLBW_Hz" ({CLBW_Hz_stepSize} Hz) and try again.'
)
break
else:
CLBW_Hz += CLBW_Hz_stepSize # Hz
# print(f'CLBW_Hz = {CLBW_Hz}')
currentKp, currentKi = get_coeffs_dc_motor_current_regulator(
R, L, CLBW_Hz)
currentKiCode = currentKi * currentKp * CL_TS
if True:
# 这里打印的用于实验中CCS的debug窗口检查电流环PI系数
上位机电流KP = CLBW_Hz
上位机电流KI = 1000
iSMC_currentKp = 上位机电流KP * L * 2 * np.pi
iSMC_currentKi = 上位机电流KI / 1000 * R / L
iSMC_currentKiCode = iSMC_currentKi * CL_TS * iSMC_currentKp
# print(f'\tiSMC_currentKp={iSMC_currentKp:g}, \
# iSMC_currentKi={iSMC_currentKi:g}, \
# iSMC_currentKiCode={iSMC_currentKiCode:g}')
# print(f'\tSimC_currentKp={currentKp:g}, \
# SimC_currentKi={currentKi:g}, \
# SimC_currentKiCode={currentKiCode:g}')
# print(f'\t上位机电流KP={上位机电流KP:g}, \
# 上位机电流KI={上位机电流KI:g}')
Gi_closed = control.tf(
[1],
[L / currentKp, 1]) # current loop zero-pole cancelled already
currentBandwidth_radPerSec = currentKp / L
# Speed loop
KT = 1.5 * n_pp * KE
dc_motor_motion = control.tf([KT * n_pp / J_total], [1, 0])
speedKp, speedKi = get_coeffs_dc_motor_SPEED_regulator(
J_total, n_pp, KE, delta, currentBandwidth_radPerSec)
speedKiCode = speedKi * speedKp * VL_TS
if True:
# 这里打印的用于实验中TI的debug窗口检查系数
上位机速度KP, 上位机速度KI = 逆上位机速度PI系数转换CODE(speedKp, speedKiCode, VL_TS,
J_total)
iSMC_speedKp, iSMC_speedKi, iSMC_speedKiCode = 上位机速度PI系数转换CODE(
上位机速度KP, 上位机速度KI, VL_TS, J_total)
# print(f'\tiSMC_speedKp={iSMC_speedKp:g}, \
# iSMC_speedKi={iSMC_speedKi:g}, \
# iSMC_speedKiCode={iSMC_speedKiCode:g}')
# print(f'\tSimC_speedKp={speedKp:g}, \
# SimC_speedKi={speedKi:g}, \
# SimC_speedKiCode={speedKiCode:g}')
# print(f'\t上位机速度KP={上位机速度KP:g}, \
# 上位机速度KI={上位机速度KI:g}')
# 下面打印的用于仿真
# print(f'\tspeedKp = {speedKp:g}', f'speedKi = {speedKi:g}', \
# f'wzero = {speedKi/2/np.pi:g} Hz', \
# f'cutoff = {delta*speedKi/2/np.pi:g} Hz', \
# f'ipole = {currentKp/L/2/np.pi:g} Hz', sep=' | ')
speedPI = control.tf([speedKp, speedKp * speedKi], [1, 0])
Gw_open = dc_motor_motion * Gi_closed * speedPI
# C2C
c2c_tf = Gi_closed
mag, phase, omega = control.bode_plot(c2c_tf,
2 * np.pi *
np.logspace(0, 4, 500),
dB=1,
Hz=1,
deg=1,
lw='0.5',
label=f'{CLBW_Hz:g} Hz')
CLBW_Hz = omega[(np.abs(mag - 0.707)).argmin()] / 2 / np.pi
C2C_designedMagPhaseOmega = mag, phase, omega
# C2V
c2v_tf = dc_motor_motion * Gi_closed
mag, phase, omega = control.bode_plot(c2v_tf,
2 * np.pi *
np.logspace(0, 4, 500),
dB=1,
Hz=1,
deg=1,
lw='0.5',
label=f'{CLBW_Hz:g} Hz')
open_cutoff_frequency_HZ = omega[(np.abs(mag -
0.0)).argmin()] / 2 / np.pi
C2V_designedMagPhaseOmega = mag, phase, omega
# V2V
Gw_closed = Gw_open / (1 + Gw_open)
mag, phase, omega = control.bode_plot(Gw_closed,
2 * np.pi *
np.logspace(0, 4, 500),
dB=1,
Hz=1,
deg=1,
lw='0.5',
label=f'{delta:g}')
VLBW_Hz = omega[(np.abs(mag - 0.707)).argmin()] / 2 / np.pi
V2V_designedMagPhaseOmega = mag, phase, omega
# print(Gw_closed)
# print('\tSpeed loop bandwidth:', VLBW_Hz, 'Hz')
return (currentKp, currentKi), \
(speedKp, speedKi), \
(上位机电流KP, 上位机电流KI), \
(上位机速度KP, 上位机速度KI), \
(C2C_designedMagPhaseOmega, C2V_designedMagPhaseOmega, V2V_designedMagPhaseOmega), \
(CLBW_Hz, VLBW_Hz, open_cutoff_frequency_HZ)
def Ftryminmax(lK, Rg, alpha, betas, beta, delta):
pos = np.array([0, 0.5 * np.pi, np.pi, 1.5 * np.pi])
omegat = np.arange(0, 360, 1)
c = np.amin(Ftrysum(lK, Rg, alpha, betas, beta, delta, omegat, pos))
d = np.amax(Ftrysum(lK, Rg, alpha, betas, beta, delta, omegat, pos))
return (np.abs(d - c))
def Ftrxminmax(lK, Rg, alpha, betas, beta, delta):
pos = np.array([0, 0.5 * np.pi, np.pi, 1.5 * np.pi])
omegat = np.arange(0, 360, 1)
a = np.amin(Ftrxsum(lK, Rg, alpha, betas, beta, delta, omegat, pos))
b = np.amax(Ftrxsum(lK, Rg, alpha, betas, beta, delta, omegat, pos))
return (np.abs(b - a))
