Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-22 08:04:07 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Cleaned up MotorSim classes

Browse Source
This commit is contained in:
PAJohnson
2020-08-18 19:44:21 -04:00
parent ead1bc3b9b
commit cbecd47051
1 changed files with 22 additions and 130 deletions
Download Patch File Download Diff File Expand all files Collapse all files
analysis/Simulation/MotorSim.py
+22 -130
View File
@@ -87,40 +87,6 @@ def rk_step(fun, t, y, f, h, A, B, C, K):
# phase_L = 0.0000157 H
# pole_pairs = 7
# KV = 270
class motor_pmsm_electrical:
# class for simulating PMSM electrical dynamics in dq reference frame
# R, L input are phase-neutral, not phase-phase
def __init__(self, R, L_q, L_d, KV, pole_pairs):
# KV is published KV
kt = 8.27/KV
self.lambda_m = 2*kt/(3*pole_pairs) #speed constant in Vs/rad (electrical rad)
self.L_q = L_q
self.L_d = L_d
self.R = R
self.pole_pairs = pole_pairs
self.V_q = 0
self.V_d = 0
self.w_e = 0
def diff_eqs(self, t, y, V_d, V_q, w):
# this is for solving with solve_ivp or similar
# t is time, y is vector of state: [I_d, I_q], V_d and V_q are input voltages in dq ref frame, w is electrical freq
I_d = y[0]
I_q = y[1]
# set these equal to the inputs for plotting
self.V_d = V_d
self.V_q = V_q
self.w_e = w
I_d_dot = V_d / self.L_d - self.R / self.L_d * I_d + w * self.L_q / self.L_d * I_q
I_q_dot = V_q / self.L_q - self.R / self.L_q * I_q - w * self.L_d / self.L_q * I_d - w * self.lambda_m / self.L_q
return np.array([I_d_dot, I_q_dot])
# example params for D5065 motor
# J = 1e-4
# b_coulomb = 0.001
# b_viscous = 0.001
@@ -142,40 +108,6 @@ class motor_pmsm_mechanical:
return np.array([theta_dot, theta_ddot])
class motor_pmsm_combined:
def __init__(self, J, b_coulomb, b_viscous, R, L_q, L_d, KV, pole_pairs):
# J is moment of inertia
# b_coulomb is coulomb friction coefficient
# b_viscous is viscous friction coefficient
self.J = J
self.b_c = b_coulomb
self.b_v = b_viscous
kt = 8.27/KV
self.lambda_m = 2*kt/(3*pole_pairs) #speed constant in Vs/rad (electrical rad)
self.L_q = L_q
self.L_d = L_d
self.R = R
self.pole_pairs = pole_pairs
def diff_eqs(self, t, y, V_d, V_q, T_load):
# inputs are V_d, V_q
# state is y, y = [theta, theta_dot, I_d, I_q]
theta = y[0]
theta_dot = y[1]
I_d = y[2]
I_q = y[3]
torque = 3*self.pole_pairs/2 * (self.lambda_m * I_q + (self.L_d - self.L_q)*I_d*I_q)
# theta_dot = theta_dot, no ode here
theta_ddot = (1/self.J) * (torque - self.b_v * theta_dot - self.b_c * sign(theta_dot))
I_d_dot = V_d / self.L_d - self.R / self.L_d * I_d + theta_dot*self.pole_pairs * self.L_q / self.L_d * I_q
I_q_dot = V_q / self.L_q - self.R / self.L_q * I_q - theta_dot*self.pole_pairs * self.L_d / self.L_q * I_d - theta_dot*self.pole_pairs * self.lambda_m / self.L_q
return np.array([theta_dot, theta_ddot, I_d_dot, I_q_dot])
def inverter(vbus, timings, current):
# this function should take the relevant inputs and output voltages in dq reference frame.
pass
@@ -193,17 +125,6 @@ class motor:
self.L_q = L_q
self.L_d = L_d
self.J = J
# set up SS matrices
# _m for mechanical, _e for electrical
self.A_m = np.array([[0,1],[0,-1*b_viscous/J]])
self.B_m = np.array([[0],[1/J]])
self.C_m = np.array([[1,0],[0,1]])
self.D_m = np.array([[0],[0]])
self.A_e = np.array([[-1*R/L_d, 0],[0, -1*R/L_q]]) # the zero terms get replaced by the theta_dot terms in simulate
self.B_e = np.array([[1/L_d, 0],[0, 1/L_q]])
self.C_e = np.array([[1,0],[0,1]])
self.D_e = np.array([[0,0],[0,0]])
# state variables for motor
self.theta = 0 # mechanical!
@@ -251,6 +172,9 @@ class motor:
torque = 3*self.pole_pairs/2 * (self.lambda_m * I_q + (self.L_d - self.L_q)*I_d*I_q) - self.T_load
if theta_dot == 0 and -1*self.b_coulomb < torque < self.b_coulomb:
torque = 0
# theta_dot = theta_dot, no ode here
theta_ddot = (1/self.J) * (torque - self.b_viscous * theta_dot - self.b_coulomb * sign(theta_dot))
I_d_dot = self.V_d / self.L_d - self.R / self.L_d * I_d + theta_dot*self.pole_pairs * self.L_q / self.L_d * I_q
@@ -266,68 +190,36 @@ class motor:
if __name__ == "__main__":
d5065 = motor(J = 1e-4, b_coulomb = 0, b_viscous = 0.01, R = 0.039, L_q = 1.57e-5, L_d = 1.57e-5, KV = 270, pole_pairs = 7, dT = 1/48000)
d5065_2 = motor_pmsm_combined(J = 1e-4, b_coulomb= 0, b_viscous = 0.01, R=0.039, L_q = 1.57e-5, L_d=1.57e-5, KV=270, pole_pairs = 7)
x0 = [0,0,0,0] # initial state of theta, theta_dot, I_d, I_q
u = [0,0,1] # input for simulation as [T_load, V_d, V_q]
t = [i*1/48000 for i in range(12000)] # half second of runtime at Fs=48kHz
#start = time.time()
data = d5065.simulate(t=t, u=u, x0=x0)
#end = time.time()
#start_ivp = time.time()
#sol = scipy.integrate.solve_ivp(d5065_2.diff_eqs, (0,0.25), t_eval=t, args=(0,1,0), y0=(0,0,0,0))
#end_ivp = time.time()
dT = 1/48000
states = []
pos = []
vel = []
I_d = []
I_q = []
#d5065.inputs(V_d = 0, V_q = 1, T_load = 0)
#start = time.time()
#for i in range(12000):
# d5065.single_step_rk(V_d = 0, V_q = 1, T_load = 0)
# pos.append(d5065.theta)
# vel.append(d5065.theta_dot)
# I_d.append(d5065.I_d)
# I_q.append(d5065.I_q)
#end = time.time()
#states = [pos, vel, I_d, I_q]
#print("rk_step time")
#print(start-end)
#print("solve_ivp time")
#print(start_ivp-end_ivp)
#print("error percentage: " + str((sol.y[0][-1] - pos[-2])/ pos[-2] * 100))
pos = data[1]
vel = data[2]
I_d = data[3]
I_q = data[4]
#pos = data[1]
#vel = data[2]
#I_d = data[3]
#I_q = data[4]
fig, axs = plt.subplots(4)
#pos_ivp = sol.y[0]
#vel_ivp = sol.y[1]
#I_d_ivp = sol.y[2]
#I_q_ivp = sol.y[3]
axs[0].plot(t, pos)
axs[0].set_title('pos')
axs[0].set_ylabel('Theta (eRad)')
axs[1].plot(t, vel)
axs[1].set_title('vel')
axs[1].set_ylabel('Omega (eRad/s)')
axs[2].plot(t,I_d)
axs[2].set_title('I_d')
axs[2].set_ylabel('Current (A)')
axs[3].plot(t,I_q)
axs[3].set_title('I_q')
axs[3].set_ylabel('Current (A)')
axs[3].set_xlabel('time (s)')
# fig, axs = plt.subplots(4)
# axs[0].plot(t, pos, linestyle=':', label='homebrew')
# axs[0].plot(t, pos_ivp, linestyle=':', label='solve_ivp')
# axs[0].set_title('pos')
# axs[0].set_ylabel('Theta (eRad)')
# axs[0].legend()
# axs[1].plot(t, vel, linestyle=':')
# axs[1].plot(t, vel_ivp, linestyle=':')
# axs[1].set_title('vel')
# axs[1].set_ylabel('Omega (eRad/s)')
# axs[2].plot(t,I_d, linestyle=':')
# axs[2].plot(t,I_d_ivp, linestyle=':')
# axs[2].set_title('I_d')
# axs[2].set_ylabel('Current (A)')
# axs[3].plot(t,I_q, linestyle=':')
# axs[3].plot(t,I_q_ivp, linestyle=':')
# axs[3].set_title('I_q')
# axs[3].set_ylabel('Current (A)')
# axs[3].set_xlabel('time (s)')
# plt.show()
plt.show()