def drawArc(self, centerxy, startxy, endxy):
v1 = np.array([startxy[0] - centerxy[0], startxy[1] - centerxy[1]])
v2 = np.array([endxy[0] - centerxy[0], endxy[1] - centerxy[1]])
cos夹角 = (v1[0] * v2[0] + v1[1] * v2[1]) / (np.sqrt(v1.dot(v1)) *
np.sqrt(v2.dot(v2)))
angle_between = np.arccos(cos夹角)
radius = np.sqrt(v1.dot(v1))
angle_start = np.arctan2(v1[1], v1[0])
angle_end = angle_start + angle_between
self.ctx.move_to(startxy[0], startxy[1])
self.ctx.arc(centerxy[0], centerxy[1], radius, angle_start, angle_end)
# self.ctx.arc_negative(centerxy[0], centerxy[1], radius, angle_start, angle_end)
return []
def gammag(lK, Rg, beta, delta, omegat):
alpha = 0.5 * np.pi - beta
cb = np.cos(beta)
sb = np.sin(beta)
cot = np.cos(np.radians(omegat))
sot = np.sin(np.radians(omegat))
cotd = np.cos(np.radians(omegat) + delta)
sotd = np.sin(np.radians(omegat) + delta)
ltx = np.cos(alpha) * cotd + cb * lK - cot * Rg
lty = sotd - sot * Rg
ltz = -np.sin(alpha) * cotd + sb * lK
Rwx = cot * Rg
Rwy = sot * Rg
Rwz = 0
lt = np.sqrt(ltx * ltx + lty * lty + ltz * ltz)
return np.degrees(np.arccos((Rwx * lty - Rwy * ltx) / (lt * Rg)))
def gammak2(lK, Rg, beta, delta, omegat):
alpha = 0.5 * np.pi - beta
cb = np.cos(beta)
sb = np.sin(beta)
cot = np.cos(np.radians(omegat))
sot = np.sin(np.radians(omegat))
cotd = np.cos(np.radians(omegat) + delta)
sotd = np.sin(np.radians(omegat) + delta)
cotdt = np.cos(np.radians(omegat + 90) + delta)
sotdt = np.sin(np.radians(omegat + 90) + delta)
ltx = np.cos(alpha) * cotd + cb * lK - cot * Rg
lty = sotd - sot * Rg
ltz = -np.sin(alpha) * cotd + sb * lK
eyrx = np.cos(alpha) * cotdt
eyry = sotdt
eyrz = -np.sin(alpha) * cotdt
lt = np.sqrt(ltx * ltx + lty * lty + ltz * ltz)
return np.degrees(np.arccos((eyrx * ltx + eyry * lty + eyrz * ltz) / lt))
def gammak(lK, Rg, beta, delta, omegat):
alpha = 0.5 * np.pi - beta
cb = np.cos(beta)
sb = np.sin(beta)
ca = np.cos(alpha)
sa = np.sin(alpha)
cot = np.cos(np.radians(omegat))
sot = np.sin(np.radians(omegat))
cotd = np.cos(np.radians(omegat) + delta)
sotd = np.sin(np.radians(omegat) + delta)
Rax = ca * cotd
Ray = sotd
Raz = -sa * cotd
ltx = Rax + cb * lK - cot * Rg
lty = Ray - sot * Rg
ltz = Raz + sb * lK
lt = np.sqrt(ltx * ltx + lty * lty + ltz * ltz)
return np.degrees(
np.arccos((sa * (Ray * ltz - Raz * lty) + ca *
(Rax * lty - Ray * ltx)) / lt))
def analyze(dot_dat_file_dir, motor_dict, sweepFreq_dict, bool_use_commanded_freq=True):
if(not os.path.exists(dot_dat_file_dir)):
print('Cannot locate:', dot_dat_file_dir)
return None
# read data as Data Frame and process
# df_info = pd.read_csv(dot_dat_file_dir+'dat/info.dat', na_values = ['1.#QNAN', '-1#INF00', '-1#IND00'])
df_profiles = pd.read_csv(dot_dat_file_dir, na_values = ['1.#QNAN', '-1#INF00', '-1#IND00'])
# print(dot_dat_file_dir)
# print(df_profiles)
no_samples = df_profiles.shape[0]
no_traces = df_profiles.shape[1]
Ts = motor_dict['CL_TS']
# print('Ts =', Ts)
# print('Simulated time: %g s.'%(no_samples * Ts * motor_dict['DOWN_SAMPLE']), 'Key list:', sep='\n')
# for key in df_profiles.keys():
# print('\t', key)
# Unpack as Series
t = np.arange(1, no_samples+1) * motor_dict['DOWN_SAMPLE'] * Ts
time = t
if sweepFreq_dict["SWEEP_FREQ_C2V"]:
# C2V
key_ref = 'CTRL.iq_cmd'; # note this is q-axis current command instead of measured q-axis current
key_qep = 'sm.omg_elec*RAD_PER_SEC_2_RPM';
x_ref = df_profiles[key_ref] # [Apk]
x_qep = df_profiles[key_qep]/60*2*np.pi*motor_dict["n_pp"] # [rpm] -> [elec.rad/s]
elif sweepFreq_dict["SWEEP_FREQ_C2C"]:
# C2C
key_ref = 'CTRL.id_cmd';
key_qep = 'ACM.id';
x_ref = df_profiles[key_ref] # [Apk]
x_qep = df_profiles[key_qep] # [Apk]
else:
# V2V
key_ref = 'ACM.rpm_cmd';
key_qep = 'sm.omg_elec*RAD_PER_SEC_2_RPM';
x_ref = df_profiles[key_ref] # [rpm] # 闭环系统传递函数分析单时候，输入输出的单位是一样的，求传递函数的时候一除，就算用的是rpm也都无所谓了（虽然理论上是按elec.rad/s设计的）
x_qep = df_profiles[key_qep] # [rpm]
# Basic DFT parameters
# N = df.shape[0]
samplingFreq = 1/Ts
EndTime = t[-1]
# print('End Time:', EndTime, 's')
# print('Sampling Frequency:', samplingFreq*1e-3, 'kHz')
# # print('Number of Points:', N)
# print()
# Plot signal in time domain
for index, value in enumerate(x_ref):
if value!=x_ref.iloc[0]:
index_begin = index
time_begin = index*Ts
break
for index, value in enumerate(x_ref[::-1]):
if value!=x_ref.iloc[-1]:
index_end = -index
time_end = EndTime - index*Ts
break
# print('index_begin', index_begin)
# print('index_end', index_end)
# print('time_begin:', time_begin, 's')
# print('time_end', time_end, 's')
# time = time [index_begin:index_end]
# x_ref = x_ref[index_begin:index_end]
# x_qep = x_qep[index_begin:index_end]
# plt.figure(100, figsize=(20,4))
# plt.title('Origianl Signal')
# plt.xlabel('Time [s]')
# plt.ylabel('Speed [elec.rad/s]')
# plt.plot(time, x_ref, label='ref')
# # plt.figure(101)
# plt.plot(time, x_qep, label='qep')
# plt.show()
# print('----------------')
# print('xref')
# for el in x_ref.values:
# print(el)
# print('xqep')
# for el in x_qep.values:
# print(el)
# # # plt.savefig(r'C:\Users\horyc\Desktop\test.png')
# quit()
# # plt.xlim([0, 8/target_Hz])
# print()
# print('Max reference speed:', max(x_ref))
# print('Max measured speed:', max(x_qep))
# print('Min reference speed:', min(x_ref))
# print('Min measured speed:', min(x_qep))
# print()
# TimeSpan = time.iloc[-1]
# print('Time Span:', TimeSpan, 's')
list_qep_max_amplitude = []
list_qep_max_frequency = []
list_qep_max_phase = []
list_ref_max_amplitude = []
list_ref_max_frequency = []
list_ref_max_phase = []
list_phase_difference = []
list_commanded_frequency = []
index_single_tone_begin = 0
index_single_tone_end = 0
# max_freq = 5 # debug
# for freq in range(2, max_freq): # datum point at 1 Hz and last point are absent in experiment
for freq in range(sweepFreq_dict['init_freq'], sweepFreq_dict['max_freq']):
period = 1.0/freq # 1.0 Duration for each frequency
index_single_tone_begin = index_single_tone_end
index_single_tone_end = index_single_tone_begin + int(period/Ts)
if False:
# use only steady state profile for this frequency
ST_time = time[int(index_single_tone_end-0.1*(index_single_tone_end-index_single_tone_begin)):index_single_tone_end]
ST_x_ref = x_ref[int(index_single_tone_end-0.1*(index_single_tone_end-index_single_tone_begin)):index_single_tone_end]
ST_x_qep = x_qep[int(index_single_tone_end-0.1*(index_single_tone_end-index_single_tone_begin)):index_single_tone_end]
else:
# use complete profile for this frequency
ST_time = time[index_single_tone_begin:index_single_tone_end]
ST_x_ref = x_ref[index_single_tone_begin:index_single_tone_end]
ST_x_qep = x_qep[index_single_tone_begin:index_single_tone_end]
# plt.plot(ST_time, ST_x_ref)
# plt.plot(ST_time, ST_x_qep)
# print(freq, index_single_tone_begin, index_single_tone_end)
if(len(ST_x_ref))<1:
print('sweep frequency too high: no data')
break
x_ref_dft = fft(ST_x_ref)
x_qep_dft = fft(ST_x_qep)
# Do DFT (Raw)
N = len(ST_time)
# plt.figure(1, figsize=(20,4))
# plt.plot(abs(x_ref_dft)/N, '.--', alpha=0.5, label='x_ref-dft');
# plt.plot(abs(x_qep_dft)/N, '.--', alpha=0.5, label='x_qep-dft');
# # plt.legend(loc='center')
# Convert raw DFT results into human-friendly forms
resolution = samplingFreq/N # [Hz]
Neff = math.ceil(N/2) # number of effective points
# 其实理论上来说，这里是比较复杂的，当N为偶数的时候，奈奎斯特频率就是采样频率的二分之一？当N为奇数的时候，还会多出一个分量，这个分量和直流分量是一对，具体我忘了……可能有错
x_ref_hat = np.append(x_ref_dft[0]/N, 2*x_ref_dft[1:Neff+1]/N) # 原始复数dft结果（双边变单边，除了直流分量，其他分量全部要乘以2）
x_qep_hat = np.append(x_qep_dft[0]/N, 2*x_qep_dft[1:Neff+1]/N)
# # Plot DFT for human to read
# plt.figure(2, figsize=(20,4))
# plt.plot(np.array(list(range(0, Neff+1)))*resolution, abs(x_ref_hat), '--s', alpha=0.5, label='ref');
# plt.plot(np.array(list(range(0, Neff+1)))*resolution, abs(x_qep_hat), '--o', alpha=0.5, label='qep');
# qep related data collection
max_amplitude = max(abs(x_qep_hat)); list_qep_max_amplitude.append(max_amplitude)
max_index = np.argmax(abs(x_qep_hat))
max_frequency = (max_index+0)*resolution; list_qep_max_frequency.append(max_frequency)
# qep phase
max_complexNumber = x_qep_hat[max_index]
max_phase = np.arctan2(max_complexNumber.imag, max_complexNumber.real); list_qep_max_phase.append(max_phase)
qep_complexNumber = max_complexNumber
# print(f'Frequency Resolution: {resolution:.2f} Hz', end=' | ')
# print(f'Max Amplitude: {max_amplitude:.2f} rpm', end=' | ')
# print(f'Corresponding Frequency: {max_frequency:.2f} Hz')
# ref related data collection
max_amplitude = max(abs(x_ref_hat)); list_ref_max_amplitude.append(max_amplitude)
max_index = np.argmax(abs(x_ref_hat))
max_frequency = (max_index+0)*resolution; list_ref_max_frequency.append(max_frequency)
# ref phase
max_complexNumber = x_ref_hat[max_index]
max_phase = np.arctan2(max_complexNumber.imag, max_complexNumber.real); list_ref_max_phase.append(max_phase)
ref_complexNumber = max_complexNumber
# phase difference
list_phase_difference.append( -np.arccos( (qep_complexNumber.real*ref_complexNumber.real+qep_complexNumber.imag*ref_complexNumber.imag)
/(abs(ref_complexNumber)*abs(qep_complexNumber)) )/np.pi*180 )
# commanded frequency
list_commanded_frequency.append(freq)
closed_loop_transfer_function = [qep/ref for ref, qep in zip(list_ref_max_amplitude, list_qep_max_amplitude)]
CL_VL_TF = [20*np.log10(el) for el in closed_loop_transfer_function]
if bool_use_commanded_freq == True:
return CL_VL_TF, list_phase_difference, list_commanded_frequency, sweepFreq_dict['max_freq']
else:
return CL_VL_TF, list_phase_difference, list_qep_max_frequency, sweepFreq_dict['max_freq']
betas = b MaRF = c MgRF = d Fakz = Ma / (MaRF * Rk) sa = np.sin(alpha) ca = np.cos(alpha) sb = np.sin(betas) cb = np.cos(betas) L = Fakz * np.sqrt(sb * sb * sa * sa + ca * ca) D = Fakz * cb * sa LoD = L / D CL = 2 * L / (rho * S * vw * vw) CD = 2 * D / (rho * S * vw * vw) CM = 2 * Ma / (rho * Rko * S * vw * vw) Cp = lam * CM aeff = np.arccos(cb * ca) Ftrxam = Ftrxminmax(lKRk, RgRk, alpha, betas, beta, delta) Ftryam = Ftryminmax(lKRk, RgRk, alpha, betas, beta, delta) h = np.sin(beta) * lK - sa * Rko print "Pnom : ", Pnom print "lK/Rk : ", lKRk print "Rg/Rk : ", RgRk print "vtip : ", vtip print "lambda: ", lam print "Ma : ", Ma print "S : ", S print "alpha : ", np.degrees(alpha) print "betas : ", np.degrees(betas) print "MaRF : ", MaRF