*

ODrive-fw-v0.5.6/analysis/Simulation/MotorSim.py

@@ -0,0 +1,225 @@
+# this file is for the simulation of a 3-phase synchronous motor
+
+import numpy as np
+import scipy as sp
+import scipy.signal as signal
+import scipy.integrate
+import matplotlib.pyplot as plt
+import time
+
+def sign(num):
+    if num > 0:
+        return 1
+    elif num < 0:
+        return -1
+    else:
+        return 0
+
+C = np.array([0, 1/5, 3/10, 4/5, 8/9, 1])
+A = np.array([
+    [0, 0, 0, 0, 0],
+    [1/5, 0, 0, 0, 0],
+    [3/40, 9/40, 0, 0, 0],
+    [44/45, -56/15, 32/9, 0, 0],
+    [19372/6561, -25360/2187, 64448/6561, -212/729, 0],
+    [9017/3168, -355/33, 46732/5247, 49/176, -5103/18656]
+])
+B = np.array([35/384, 0, 500/1113, 125/192, -2187/6784, 11/84])
+
+# rk_step from scipy.integrate rk.py
+def rk_step(fun, t, y, f, h, A, B, C, K):
+    """Perform a single Runge-Kutta step.
+    This function computes a prediction of an explicit Runge-Kutta method and
+    also estimates the error of a less accurate method.
+    Notation for Butcher tableau is as in [1]_.
+    Parameters
+    ----------
+    fun : callable
+        Right-hand side of the system.
+    t : float
+        Current time.
+    y : ndarray, shape (n,)
+        Current state.
+    f : ndarray, shape (n,)
+        Current value of the derivative, i.e., ``fun(x, y)``.
+    h : float
+        Step to use.
+    A : ndarray, shape (n_stages, n_stages)
+        Coefficients for combining previous RK stages to compute the next
+        stage. For explicit methods the coefficients at and above the main
+        diagonal are zeros.
+    B : ndarray, shape (n_stages,)
+        Coefficients for combining RK stages for computing the final
+        prediction.
+    C : ndarray, shape (n_stages,)
+        Coefficients for incrementing time for consecutive RK stages.
+        The value for the first stage is always zero.
+    K : ndarray, shape (n_stages + 1, n)
+        Storage array for putting RK stages here. Stages are stored in rows.
+        The last row is a linear combination of the previous rows with
+        coefficients
+    Returns
+    -------
+    y_new : ndarray, shape (n,)
+        Solution at t + h computed with a higher accuracy.
+    f_new : ndarray, shape (n,)
+        Derivative ``fun(t + h, y_new)``.
+    References
+    ----------
+    .. [1] E. Hairer, S. P. Norsett G. Wanner, "Solving Ordinary Differential
+           Equations I: Nonstiff Problems", Sec. II.4.
+    """
+    K[0] = f
+    for s, (a, c) in enumerate(zip(A[1:], C[1:]), start=1):
+        dy = np.dot(K[:s].T, a[:s]) * h
+        K[s] = fun(t + c * h, y + dy)
+
+    y_new = y + h * np.dot(K[:-1].T, B)
+    f_new = fun(t + h, y_new)
+
+    K[-1] = f_new
+
+    return y_new, f_new
+
+
+# example params for d5065 motor
+# phase_R = 0.039 Ohms
+# phase_L = 0.0000157 H
+# pole_pairs = 7
+# KV = 270
+# J = 1e-4
+# b_coulomb = 0.001
+# b_viscous = 0.001
+
+class motor_pmsm_mechanical:
+    def __init__(self, J, b_coulomb, b_viscous):
+        # 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
+
+    def diff_eqs(self, t, y, torque):
+        theta = y[0]
+        theta_dot = y[1]
+
+        theta_ddot = (1/self.J) * (torque - self.b_v * theta_dot - self.b_c * sign(theta_dot))
+
+        return np.array([theta_dot, theta_ddot])
+
+def inverter(vbus, timings, current):
+    # this function should take the relevant inputs and output voltages in dq reference frame.
+    pass
+
+class motor:
+    def __init__(self, J, b_coulomb, b_viscous, R, L_q, L_d, KV, pole_pairs, dT):
+        self.dT = dT
+        self.b_coulomb = b_coulomb
+        self.b_viscous = b_viscous
+        self.KV = KV
+        self.pole_pairs = pole_pairs
+        kt = 8.27/KV
+        self.lambda_m = 2*kt/(3*pole_pairs) #speed constant in Vs/rad (electrical rad)
+        self.R = R
+        self.L_q = L_q
+        self.L_d = L_d
+        self.J = J
+
+        # state variables for motor
+        self.theta = 0      # mechanical!
+        self.theta_dot = 0  # mechanical!
+        self.I_d = 0
+        self.I_q = 0
+
+        # K matrix. For integrator?
+        # np.empty((self.n_stages + 1, self.n_stages), dtype=self.y.dtype)
+        self.K = np.empty((7, 4))
+
+    def simulate(self, t, u, x0):
+        # t is timesteps [t0, t1, ...]
+        # u is [T_load, V_d, V_q]
+        # x0 is initial states, [theta, theta_dot, I_d, I_q]
+        (self.theta, self.theta_dot, self.I_d, self.I_q) = x0
+        time = []
+        pos = []
+        vel = []
+        I_d = []
+        I_q = []
+        for i in range(len(t)):
+            self.single_step_rk(u[2],u[1],u[0])
+            time.append(i*self.dT)
+            pos.append(self.theta)
+            vel.append(self.theta_dot)
+            I_d.append(self.I_d)
+            I_q.append(self.I_q)
+
+        return [time,pos,vel,I_d,I_q]
+
+    def inputs(self, V_q, V_d, T_load):
+        self.V_q = V_q
+        self.V_d = V_d
+        self.T_load = T_load
+
+    def diff_eqs(self, t, y):
+        # inputs are self.V_q, self.V_d, self.T_load
+        # state is y, y = [theta, theta_dot, I_d, I_q]
+        # set_inputs must be called before this if the inputs have changed.
+        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) - 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
+        I_q_dot = self.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 single_step_rk(self, V_q, V_d, T_load):
+        # given inputs
+        self.inputs(V_q, V_d, T_load)
+        x = (d5065.theta, d5065.theta_dot, d5065.I_d, d5065.I_q)
+        ((d5065.theta, d5065.theta_dot, d5065.I_d, d5065.I_q), _) = rk_step(d5065.diff_eqs, 0, x, d5065.diff_eqs(0, x), d5065.dT, A, B, C, d5065.K)
+
+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)
+    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
+
+    data = d5065.simulate(t=t, u=u, x0=x0)
+    dT = 1/48000
+    states = []
+    pos = []
+    vel = []
+    I_d = []
+    I_q = []
+    pos = data[1]
+    vel = data[2]
+    I_d = data[3]
+    I_q = data[4]
+
+    fig, axs = plt.subplots(4)
+
+    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)')
+
+    plt.show()

ODrive-fw-v0.5.6/analysis/Simulation/TranslationalMass.py

@@ -0,0 +1,62 @@
+import os
+import matplotlib.pyplot as plt
+from control.matlab import *
+import numpy as np
+
+
+# Input: Current (A)
+# Output: Torque (Nm)
+# Params: Kt (Nm/A)
+def motor(Kt):
+    return tf(Kt, 1)
+
+# Mass-Spring-Damper
+# Input: Force
+# Output: Position
+# Params:   m (kg)
+#           b 
+#           k (N/m)
+def mass(m, b, k):
+    A = [[0, 1.], [-k/m, -b/m]]
+    B = [[0], [1/m]]
+    C = [[1., 0]]
+    return ss(A, B, C, 0)
+
+# Input: Torque (Nm)
+# Output: Force (N)
+# Params: r (m)
+def pulley(r):
+    return tf(r, 1)
+
+# Make s a transfer function s/1
+s = tf('s')
+print(s)
+
+# build a new transfer function using our variable s as a handy placeholder
+sys = 1 / (s*s + s + 1)
+print(sys)
+
+# Hit the system with a step command
+yout, T = step(sys)
+plt.plot(T, yout)
+
+# convert our continuous time model to discrete time via Tustin at 0.01s timestep
+sysd = c2d(tf(sys), 0.01, method='tustin')
+print(sysd)
+
+# Hit the discrete system with a step command, and sample it at 0.01 timestep from 0 to 14 seconds
+yout, T = step(sysd, np.arange(0, 14, 0.01))
+plt.plot(T, yout)
+plt.legend(['Continuous', 'Discrete'])
+
+# Build a system based on the series connection of the motor, pulley, and mass "blocks"
+sys = series(motor(2.5), pulley(0.015), mass(0.10, .1, .1))
+print(tf(sys))
+
+# Step our series system, returning y (outputs) and x (states)
+yout, T, xout = step(sys, return_x=True)
+plt.figure()
+plt.plot(T, yout)
+plt.plot(T, xout)
+plt.legend(['Displacement', r'$x$', r'$\dot{x}$'])
+plt.show()

ODrive-fw-v0.5.6/analysis/cogging_torque/cogging_harmonics.py

@@ -0,0 +1,52 @@
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+encoder_cpr = 2400
+stator_slots = 12
+pole_pairs = 7
+
+N = data.size
+fft = np.fft.rfft(data)
+freq = np.fft.rfftfreq(N, d=1./encoder_cpr)
+
+harmonics = [0]
+harmonics += [(i+1)*stator_slots for i in range(pole_pairs)]
+harmonics += [pole_pairs]
+harmonics += [(i+1)*2*pole_pairs for i in range(int(stator_slots/4))]
+
+
+fft_sparse = fft.copy()
+indicies = np.arange(fft_sparse.size)
+mask = [i not in harmonics for i in indicies]
+fft_sparse[mask] = 0.0
+interp_data = np.fft.irfft(fft_sparse)
+
+#%%
+
+#plt.figure()
+plt.subplot(3, 1, 1)
+plt.plot(data, label='raw')
+plt.plot(interp_data, label='selected harmonics IFFT')
+plt.title('cogging map')
+plt.xlabel('counts')
+plt.ylabel('A')
+plt.legend(loc='best')
+
+#plt.figure()
+plt.subplot(3, 1, 2)
+plt.stem(freq, np.abs(fft)/N, label='raw')
+plt.stem(freq[harmonics], np.abs(fft_sparse[harmonics])/N, markerfmt='ro', label='selected harmonics')
+plt.title('cogging map spectrum')
+plt.xlabel('cycles/turn')
+plt.ylabel('A')
+plt.legend(loc='best')
+
+#plt.figure()
+plt.subplot(3, 1, 3)
+plt.stem(freq, np.abs(fft)/N, label='raw')
+plt.stem(freq[harmonics], np.abs(fft_sparse[harmonics])/N, markerfmt='ro', label='selected harmonics')
+plt.title('cogging map spectrum')
+plt.xlabel('cycles/turn')
+plt.ylabel('A')
+plt.legend(loc='best')

ODrive-fw-v0.5.6/analysis/filterpoles.py

@@ -0,0 +1,90 @@
+
+import numpy as np
+import sympy as sp
+from scipy.integrate import solve_ivp
+import matplotlib.pyplot as plt
+
+do_mass_spring = True
+do_PLL = False
+
+bandwidth = 10
+
+pos_ref = 0
+vel_ref = 0
+init_pos = 1000
+init_vel = 0
+
+plotend = 1
+plotfrequency = 1000.0
+
+fig, ax1 = plt.subplots()
+
+if do_mass_spring:
+    # 2nd order system response with manipulation of velocity only
+    # This is similar to a mass/spring/damper system
+    # pos_dot = vel
+    # vel_dot = Kp * delta_pos + Ki * delta_vel
+
+    Ki = 2.0 * bandwidth
+    Kp = 0.25 * Ki**2
+
+    def get_Xdot(t, X):
+        pos = X[0]
+        vel = X[1]
+
+        pos_err = pos_ref - pos
+        vel_err = vel_ref - vel
+        
+        pos_dot = vel
+        vel_dot = Kp * pos_err + Ki * vel_err
+
+        Xdot = [pos_dot, vel_dot]
+        return Xdot
+
+    sol = solve_ivp(get_Xdot, (0.0, plotend), [init_pos, init_vel], t_eval=np.linspace(0, plotend, plotend*plotfrequency))
+
+    color = 'tab:red'
+    ax1.set_xlabel('time (s)')
+    ax1.set_ylabel('pos', color=color)
+    ax1.plot(np.transpose(sol.t), np.transpose(sol.y[0,:]), label='physical mass', color=color)
+    ax1.tick_params(axis='y', labelcolor=color)
+
+    ax2 = ax1.twinx()  # instantiate a second axes that shares the same x-axis
+
+    color = 'tab:blue'
+    ax2.set_ylabel('vel', color=color)  # we already handled the x-label with ax1
+    ax2.plot(np.transpose(sol.t), np.transpose(sol.y[1,:]), label='physical mass', color=color)
+    ax2.tick_params(axis='y', labelcolor=color)
+
+
+if do_PLL:
+    # 2nd order system response with a "slipping displacement" term directly on position
+    # This formulation is given in the sensorless PLL paper
+    # pos_dot = vel + Kp * delta_pos
+    # vel_dot = Ki * delta_pos
+
+    Kp = 2.0 * bandwidth
+    Ki = 0.25 * Kp**2
+
+    def get_Xdot(t, X):
+        pos = X[0]
+        vel = X[1]
+
+        pos_err = pos_ref - pos
+        vel_err = vel_ref - vel
+
+        pos_dot = vel + Kp * pos_err
+        vel_dot = Ki * pos_err
+
+        Xdot = [pos_dot, vel_dot]
+        return Xdot
+
+    sol = solve_ivp(get_Xdot, (0.0, plotend), [init_pos, init_vel], t_eval=np.linspace(0, plotend, plotend*plotfrequency))
+
+    plt.plot(np.transpose(sol.t), np.transpose(sol.y[0,:]), label='PLL pos')
+    plt.plot(np.transpose(sol.t), np.transpose(sol.y[1,:]), label='PLL vel')
+
+
+
+plt.legend()
+plt.show(block=True)

ODrive-fw-v0.5.6/analysis/motor_analysis/VelliPlot.m

@@ -0,0 +1,98 @@
+% limits
+Imax = 64; %A
+Umax = 22; %V
+Irange = 100; %A. Range for plotting current
+omegaMax = 2000; %rad/s mechanical. For plotting voltage ellipses
+omegastep = 200; %rad/s mechanical. For plotting voltage ellipses
+
+
+%%
+%350 kv motor
+lambda = 2.24/1000;
+L = 23e-6;
+R = 32e-3;
+pp = 7; Poles = pp*2;
+Ld = L;
+Lq = L;
+
+% %%
+% % Donkey
+% kv = 820;
+% lambda = 60/(kv*2*pi*pp*sqrt(3));
+% L = 8e-6; %Guess! TODO: measure
+% R = 30e-3; %Guess! TODO: measure
+% pp = 7; Poles = pp*2;
+% Ld = L;
+% Lq = L;
+
+%%
+Istep = Irange/400;
+Idplt = repmat(-Irange:Istep:Irange,801,1);
+Iqplt = repmat((-Irange:Istep:Irange)',1,801);
+UmaxSq = (Umax/sqrt(3))^2;
+
+t = linspace(0,2*pi);
+IdMaxt = Imax*cos(t);
+IqMaxt = Imax*sin(t);
+
+%%
+figure(1)
+plot(IdMaxt, IqMaxt);
+
+hold on;
+
+%Plot torque
+%[c,h] = contour(Idplt,Iqplt, (Poles/2).*(3/2).*(lambda.*Iqplt + (Ld-Lq).*Iqplt.*Idplt), -5:0.25:5);
+%clabel(c,h,'LabelSpacing',500);
+
+%Plot voltage ellipses
+EllRHS = Ld^2.*(lambda/Ld + Idplt).^2 + Lq^2.*Iqplt.^2;
+omegaAtEllipse = sqrt(UmaxSq./EllRHS);
+[c,h] = contour(Idplt,Iqplt, omegaAtEllipse./(Poles/2), 0:omegastep:omegaMax);
+clabel(c,h,'LabelSpacing',500);
+xlabel 'Id (A)'
+ylabel 'Iq (A)'
+colormap(jet)
+c = colorbar;
+%c.Label.String = 'Speed (rad/s)';
+ylabel(c,'Speed (mechanical rad/s)')
+grid on
+axis equal
+
+%plot infinite speed point
+plot(-lambda/Ld, 0, 'r*');
+
+hold off;
+
+%%
+figure(2)
+
+%fw range
+t = linspace(pi/2,pi);
+IdMaxt = Imax*cos(t);
+IqMaxt = Imax*sin(t);
+Tmaxt = (Poles/2).*(3/2).*(lambda.*IqMaxt + (Ld-Lq).*IqMaxt.*IdMaxt);
+EllRHSmaxt = Ld^2.*(lambda/Ld + IdMaxt).^2 + Lq^2.*IqMaxt.^2;
+omegamaxt = sqrt(UmaxSq./EllRHSmaxt);
+
+%MTPA range
+MTPAmaxtId = 0; %TODO make work for salient machines
+MTPAmaxtIq = Imax;
+TmaxtMTPA = (Poles/2).*(3/2).*(lambda.*MTPAmaxtIq + (Ld-Lq).*MTPAmaxtIq.*MTPAmaxtId);
+Tmaxt = [repmat(TmaxtMTPA, 1, 100) Tmaxt];
+omegamaxt = [linspace(0,omegamaxt(1)) omegamaxt];
+
+%Present mechanical speed
+omegamaxt = omegamaxt./(Poles/2);
+
+Pmaxt = Tmaxt.*omegamaxt;
+
+%plotyy(t, Tmaxt, t, omegamaxt);
+%plotyy(t, Tmaxt, t, Pmaxt);
+%plotyy(t, Pmaxt, t, omegamaxt);
+
+h = plotyy(omegamaxt, Tmaxt, omegamaxt, Pmaxt);
+grid on
+xlabel 'Speed (mechanical rad/s)'
+ylabel 'Torque (Nm)'
+ylabel(h(2), 'Power (W)');

ODrive-fw-v0.5.6/analysis/motor_analysis/ac_induction_motor.py

@@ -0,0 +1,249 @@
+
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy.integrate import solve_ivp
+from scipy.optimize import least_squares
+from engineering_notation import EngNumber
+
+filename = "oscilloscope.csv"
+
+USE_TEST_DATA = False
+PLOT_INITAL = True
+DO_FITTING = False
+PLOT_PROGRESS = False
+REPORT_PROGRESS = True
+assumed_rotor_resistance = 1
+pole_pairs = 2
+
+class ACMotor():
+    """
+    Models an induction motor based on Eq 10 in [1].
+    [1] https://pdfs.semanticscholar.org/4770/15e472da4c2e05e9ff8c1b921c76a938f786.pdf
+
+
+    Note: This model refers all rotor quantities to the stator, i.e. the
+    quantities are as if the motor had a winding ratio of k = 1.
+    """
+
+    # parameters: (name, range)
+    parameter_definitions = [
+        ('stator_inductance', (0, np.inf), 'H'), # aka l_s, [Henry]
+        ('stator_resistance', (0, np.inf), 'ohm'), # aka r_s, [Ohm]
+        ('rotor_inductance',  (0, np.inf), 'H'), # aka l_r [Henry]
+        # ('rotor_resistance',  (0, np.inf), 'ohm'), # aka r_r [Ohm]
+        ('mutual_inductance_factor', (0, 1.0), ''), #[unitless] = l_m**2 / (l_s * l_r)
+    ]
+    # parameter index lookup
+    pl = {r[0]:i for i, r in enumerate(parameter_definitions)}
+
+    # states: (name, initial_value)
+    state_definitions = [
+        ('stator_current', 0.0), # aka i_s, [A]
+        ('rotor_flux', 0.0),     # aka Phi_r, [Wb]
+    ] # complex numbers
+    # state index lookup
+    sl = {r[0]:i for i, r in enumerate(state_definitions)}
+
+    def __init__(self, params):
+        self.params = params
+
+        # Assigned in run():
+        # self.stator_voltage = None
+        # self.omega_stator = None
+        # self.omega_rotor = None
+
+    def get_mutual_inductance(self):
+        return np.sqrt(
+            self.params[ACMotor.pl['mutual_inductance_factor']]
+            * self.params[ACMotor.pl['stator_inductance']]
+            * self.params[ACMotor.pl['rotor_inductance']]
+        )
+
+    def system_function(self, t, y):
+        # local shorthand for params
+        p = self.params
+        pl = ACMotor.pl
+        sl = ACMotor.sl
+
+        # rotor_resistance = p[pl['rotor_resistance']] 
+        rotor_resistance = assumed_rotor_resistance
+        mutual_inductance = self.get_mutual_inductance()
+
+        tau_rotor = p[pl['rotor_inductance']] / rotor_resistance # [s]
+        coupling_factor = mutual_inductance / p[pl['rotor_inductance']] # aka k_r [unitless]
+        r_sigma = p[pl['stator_resistance']] + coupling_factor**2 * rotor_resistance # [Ohm]
+        leakage_factor = 1.0 - mutual_inductance**2 / (p[pl['rotor_inductance']] * p[pl['stator_inductance']]) # aka sigma [unitless]
+        tau_stator_prime = leakage_factor * p[pl['stator_inductance']] / r_sigma # [s]
+
+        # [1] Eq 10a
+        dstator_current_dt = (
+                -1.0j * self.omega_stator * tau_stator_prime * y[sl['stator_current']]
+                - coupling_factor / (r_sigma * tau_rotor) * (1.0j*self.omega_rotor * tau_rotor - 1.0) * y[sl['rotor_flux']]
+                + 1.0 / r_sigma * self.stator_voltage
+                - y[sl['stator_current']]
+            ) / tau_stator_prime
+
+        # [1] Eq 10b
+        drotor_flux_dt = (
+                -1.0j * (self.omega_stator - self.omega_rotor) * tau_rotor * y[sl['rotor_flux']]
+                + mutual_inductance * y[sl['stator_current']]
+                - y[sl['rotor_flux']]
+            ) / tau_rotor
+
+        return [dstator_current_dt, drotor_flux_dt]
+
+    def run(self, time_series, voltage, omega_stator, omega_rotor):
+        self.stator_voltage = voltage
+        self.omega_stator = omega_stator
+        self.omega_rotor = omega_rotor
+
+        y0 = np.array([x[1] for x in ACMotor.state_definitions], dtype=np.complex)
+
+        result = solve_ivp(self.system_function, (time_series[0], time_series[-1]), y0, t_eval=time_series)
+        y = result.y
+
+        # compute derived state
+        rotor_inductance = self.params[ACMotor.pl['rotor_inductance']]
+        rotor_current = (1/rotor_inductance) * (y[1] - self.get_mutual_inductance() * y[0])
+        return np.vstack((y, rotor_current))
+
+    def print_parameter_info(self):
+        print()
+        print('Given parameters:')
+        print('pole_pairs = {}'.format(EngNumber(pole_pairs)))
+        print('rotor_resistance = {}ohm'.format(EngNumber(assumed_rotor_resistance)))
+
+        print()
+        print('Fitted parameters:')
+        for i, r in enumerate(ACMotor.parameter_definitions):
+            print('{} = {}{}'.format(r[0], EngNumber(self.params[i]), r[2]))
+
+        print()
+        print('Derived parameters:')
+        mutual_inductance = motor.get_mutual_inductance()
+        coupling_factor = mutual_inductance / self.params[ACMotor.pl['rotor_inductance']]
+        torque_constant = pole_pairs * coupling_factor * mutual_inductance
+        motor_constant = torque_constant / (3.0 * self.params[ACMotor.pl['stator_resistance']])
+        print('mutual_inductance = {}H'.format(EngNumber(mutual_inductance)))
+        print('coupling_factor = {}'.format(EngNumber(coupling_factor)))
+        print('torque_constant = {}Nm/A^2'.format(EngNumber(torque_constant)))
+        print('motor_constant = {}Nm/W'.format(EngNumber(motor_constant)))
+
+    def print_run_info(self, y):
+        final_stator_current_d = np.real(y[0,-1])
+        final_stator_current_q = np.imag(y[0,-1])
+        final_rotor_flux_d = np.real(y[1,-1])
+
+        mutual_inductance = self.get_mutual_inductance()
+        coupling_factor = mutual_inductance / self.params[ACMotor.pl['rotor_inductance']]
+        final_torque_per_q_amp = pole_pairs * coupling_factor * final_rotor_flux_d
+
+        print()
+        print('Final values:')
+        print('final_rotor_flux_d = {}Wb'.format(EngNumber(final_rotor_flux_d)))
+        print('final_stator_current_d = {}A'.format(EngNumber(final_stator_current_d)))
+        print('final_stator_current_q = {}A'.format(EngNumber(final_stator_current_q)))
+        print('final_torque_per_q_amp = {}Nm/A'.format(EngNumber(final_torque_per_q_amp)))
+
+def plot_data(t, y, ref, title):
+    fig, (ax1, ax2) = plt.subplots(2, sharex=True)
+    ax1b = ax1.twinx()
+    ax1.plot(t, ref, label='Measured current')
+    ax1.plot(t, np.real(y[0]), label='Stator current (d)')
+    ax1.plot(t, np.imag(y[0]), label='Stator current (q)')
+    ax2.plot(t, np.real(y[2]), label='Rotor current (d)')
+    ax2.plot(t, np.imag(y[2]), label='Rotor current (q)')
+    ax1b.plot(t, 1000*np.real(y[1]), 'C3', label='Rotor flux (d)')
+    ax1b.plot(t, 1000*np.imag(y[1]), 'C4', label='Rotor flux (q)')
+    ax1.set_xlabel('time [s]')
+    ax1.set_ylabel('Current [A]')
+    ax1b.set_ylabel('Flux [mWb]')
+    ax2.set_ylabel('Current [A]')
+    plt.title(title)
+    fig.legend()
+    plt.show()
+
+# load test data
+t = np.arange(4096)/8000.0
+voltage_step = 1.0
+if USE_TEST_DATA:
+    with open(filename, 'r') as fp:
+        test_response = np.array([float(x) for x in fp.readlines()])
+else: test_response = None
+
+
+inital_parameters = np.zeros(len(ACMotor.parameter_definitions))
+inital_parameters[ACMotor.pl['stator_inductance']] = 7.72181086e-04
+inital_parameters[ACMotor.pl['stator_resistance']] = 3.06884624e-02
+inital_parameters[ACMotor.pl['rotor_inductance']] = assumed_rotor_resistance*6.82013522e-02
+# inital_parameters[ACMotor.pl['rotor_resistance']] = 1.0e-0
+# inital_parameters[ACMotor.pl['mutual_inductance']] = 2.40e-4
+inital_parameters[ACMotor.pl['mutual_inductance_factor']] = 8.68671978e-01
+
+# inital_parameters[ACMotor.pl['stator_resistance']] = 1.298
+# inital_parameters[ACMotor.pl['stator_inductance']] = 0.157228647
+# inital_parameters[ACMotor.pl['rotor_resistance']] = 0.975052932
+# inital_parameters[ACMotor.pl['rotor_inductance']] = 0.16674423623999998
+# inital_parameters[ACMotor.pl['mutual_inductance']] = 0.157221177
+
+# Plot initial run
+if PLOT_INITAL:
+    print()
+    print('Initial run:')
+    motor = ACMotor(inital_parameters)
+    motor.print_parameter_info()
+
+    y = motor.run(
+        time_series = t,
+        voltage = voltage_step,
+        omega_stator = 0,
+        omega_rotor= 0)
+    motor.print_run_info(y)
+    plot_data(t, y, test_response, 'initial')
+
+
+# Fit to data
+def get_residuals(params):
+    if REPORT_PROGRESS: print(params)
+    motor = ACMotor(params)
+    y = motor.run(
+        time_series = t,
+        voltage = voltage_step,
+        omega_stator = 0,
+        omega_rotor= 0)
+    
+    residuals = test_response - np.real(y[0])
+    fitness = sum(residuals**2)
+    if REPORT_PROGRESS: print(fitness)
+
+    if PLOT_PROGRESS:
+        plot_data(t, y, test_response, 'progress')
+    
+    return residuals
+
+if DO_FITTING:
+    print()
+    print('Fitting parameters:')
+    optiresult = least_squares(get_residuals, inital_parameters,
+        bounds=list(zip(*[x[1] for x in ACMotor.parameter_definitions])),
+        x_scale='jac',
+        diff_step = 1e-2 * np.array([
+            7.58192590e-04,
+            3.07166671e-02,
+            6.85207075e-02,
+            # 4.66461518e+00,
+            8.67540012e-01])
+    )
+    print(optiresult.message)
+
+    motor = ACMotor(optiresult.x)
+    motor.print_parameter_info()
+
+    y = motor.run(
+        time_series = t,
+        voltage = voltage_step,
+        omega_stator = 0,
+        omega_rotor= 0)
+    motor.print_run_info(y)
+    plot_data(t, y, test_response, 'final')
+

ODrive-fw-v0.5.6/analysis/numeric_path_opt/Main.m

@@ -0,0 +1,43 @@
+%Params
+AccelPerA = 3000;
+phaseR = 0.033;
+Ts = 0.001;
+N = 50;
+thetaFinal = 200;
+x0 = [0;0];
+
+%system definition
+%X = [theta; omega]
+Ac = [0 1;
+     0 0];
+Bc = [0;
+     AccelPerA];
+C = 0;
+D = 0;
+
+SYSC = ss(Ac, Bc, [], []);
+SYSD = c2d(SYSC, Ts, 'zoh');
+[Phi, Gamma] = predictionmatrices(SYSD.a, SYSD.b, SYSD.c, N);
+
+Df = Gamma(end-1:end,:);
+ff = [thetaFinal; 0];
+
+H = 2*eye(N)*(3/2)*phaseR*Ts;
+[u, fval] = quadprog(H,[],[],[],Df,ff);
+
+xv = reshape(Gamma*u, 2,N)';
+x = xv(:,1);
+v = xv(:,2);
+
+kv350_lambda = 2.2e-3;
+power = u.*v.*(3/2)*kv350_lambda;
+
+Vbus = 24;
+Ib = power/Vbus;
+Im = u;
+duty = Ib./Im;
+CapIsqr = (duty.*(Ib-Im)).^2 + ((1-duty).*Ib).^2;
+CapIrms = sqrt(sum(CapIsqr)/N);
+CapR = 0.08;
+Ncaps = 8;
+Cappow = (CapIrms/Ncaps)^2 * CapR

ODrive-fw-v0.5.6/analysis/numeric_path_opt/predictionmatrices.m

@@ -0,0 +1,14 @@
+function [Phi, Gamma, Lambda] = predictionmatrices(A, B, C, N)
+%UNTITLED2 Summary of this function goes here
+%   Detailed explanation goes here
+
+n = size(A,1);
+Atilde = [A; zeros((N-1)*n, n)];
+k = [zeros(n, N*n); -kron(eye(N-1), A) zeros((N-1)*n,n)] + eye(N*n);
+
+Phi = k\Atilde;
+Gamma = k\kron(eye(N), B);
+Lambda = kron(eye(N), C);
+
+end
+

ODrive-fw-v0.5.6/analysis/thermistors.py

@@ -0,0 +1,9 @@
+#%%
+from odrive.utils import calculate_thermistor_coeffs
+
+Rload = 3300 # 2000 for ODrive v4
+R_25 = 10000
+Beta = 3434
+Tmin = 0
+Tmax = 140
+calculate_thermistor_coeffs(3, Rload, R_25, Beta, Tmin, Tmax, True)