motors/python/haptic_phase_current.py

#!/usr/bin/python3

import numpy
from matplotlib import pylab
import scipy.integrate
from frc971.control_loops.python import controls
import time
import operator

K1 = 1.81e04
K2 = 0.0

# Make the amplitude of the fundamental 1 for ease of playing with.
K2 /= K1
K1 = 1

vcc = 14.0  # volts
R_motor = 0.1073926073926074  # ohms for the motor
R = R_motor + 0.080 + 0.02  # motor + fets + wires ohms for system

L = 80.0 * 1e-6  # Henries
M = L / 10.0

Kv = 37.6  # rad/s/volt, where the voltage is measured from the neutral to the phase.
J = 0.0000007

R_shunt = 0.0003

# RC circuit for current sense filtering.
R_sense1 = 768.0
R_sense2 = 1470.0
C_sense = 10.0 * 1e-9

# So, we measured the inductance by switching between ~5 and ~20 amps through
# the motor.
# We then looked at the change in voltage that should give us (assuming duty
# cycle * vin), and divided it by the corresponding change in current.

# We then looked at the amount of time it took to decay the current to 1/e
# That gave us the inductance.

# Overrides for experiments
J = J * 10.0

# Firing phase A -> 0.0
# Firing phase B -> - numpy.pi * 2.0 / 3.0
# Firing phase C -> + numpy.pi * 2.0 / 3.0

hz = 20000.0

#switching_pattern = 'front'
switching_pattern = 'centered'
#switching_pattern = 'rear'
#switching_pattern = 'centered front shifted'
#switching_pattern = 'anticentered'

Vconv = numpy.matrix([[2.0, -1.0, -1.0], [-1.0, 2.0, -1.0], [-1.0, -1.0, 2.0]
                      ]) / 3.0


def f_single(theta):
    return K1 * numpy.sin(theta) + K2 * numpy.sin(theta * 5)


def g_single(theta):
    return K1 * numpy.sin(theta) - K2 * numpy.sin(theta * 5)


def gdot_single(theta):
    """Derivitive of the current.

  Must be multiplied by omega externally.
  """
    return K1 * numpy.cos(theta) - 5.0 * K2 * numpy.cos(theta * 5.0)


f = numpy.vectorize(f_single, otypes=(numpy.float, ))
g = numpy.vectorize(g_single, otypes=(numpy.float, ))
gdot = numpy.vectorize(gdot_single, otypes=(numpy.float, ))


def torque(theta):
    return f(theta) * g(theta)


def phase_a(function, theta):
    return function(theta)


def phase_b(function, theta):
    return function(theta + 2 * numpy.pi / 3)


def phase_c(function, theta):
    return function(theta + 4 * numpy.pi / 3)


def phases(function, theta):
    return numpy.matrix([[phase_a(function,
                                  theta)], [phase_b(function, theta)],
                         [phase_c(function, theta)]])


def all_phases(function, theta_range):
    return (phase_a(function, theta_range) + phase_b(function, theta_range) +
            phase_c(function, theta_range))


theta_range = numpy.linspace(start=0, stop=4 * numpy.pi, num=10000)
one_amp_driving_voltage = R * g(theta_range) + (
    L * gdot(theta_range) + M * gdot(theta_range + 2.0 / 3.0 * numpy.pi) +
    M * gdot(theta_range - 2.0 / 3.0 * numpy.pi)) * Kv * vcc / 2.0

max_one_amp_driving_voltage = max(one_amp_driving_voltage)

# The number to divide the product of the unit BEMF and the per phase current
# by to get motor current.
one_amp_scalar = (phases(f_single, 0.0).T * phases(g_single, 0.0))[0, 0]

print 'Max BEMF', max(f(theta_range))
print 'Max current', max(g(theta_range))
print 'Max drive voltage (one_amp_driving_voltage)', max(
    one_amp_driving_voltage)
print 'one_amp_scalar', one_amp_scalar

pylab.figure()
pylab.subplot(1, 1, 1)
pylab.plot(theta_range, f(theta_range), label='bemf')
pylab.plot(theta_range, g(theta_range), label='phase_current')
pylab.plot(theta_range, torque(theta_range), label='phase_torque')
pylab.plot(theta_range,
           all_phases(torque, theta_range),
           label='sum_torque/current')
pylab.legend()


def full_sample_times(Ton, Toff, dt, n, start_time):
    """Returns n + 4 samples for the provided switching times.

  We need the timesteps and Us to integrate.

  Args:
    Ton: On times for each phase.
    Toff: Off times for each phase.
    dt: The cycle time.
    n: Number of intermediate points to include in the result.
    start_time: Starting value for the t values in the result.

  Returns:
    array of [t, U matrix]
  """

    assert ((Toff <= 1.0).all())
    assert ((Ton <= 1.0).all())
    assert ((Toff >= 0.0).all())
    assert ((Ton >= 0.0).all())

    if (Ton <= Toff).all():
        on_before_off = True
    else:
        # Verify that they are all ordered correctly.
        assert (not (Ton <= Toff).any())
        on_before_off = False

    Toff = Toff.copy() * dt
    Toff[Toff < 100e-9] = -1.0
    Toff[Toff > dt] = dt

    Ton = Ton.copy() * dt
    Ton[Ton < 100e-9] = -1.0
    Ton[Ton > dt - 100e-9] = dt + 1.0

    result = []
    t = 0

    result_times = numpy.concatenate(
        (numpy.linspace(0, dt, num=n),
         numpy.reshape(
             numpy.asarray(Ton[numpy.logical_and(Ton < dt, Ton > 0.0)]),
             (-1, )),
         numpy.reshape(
             numpy.asarray(Toff[numpy.logical_and(Toff < dt, Toff > 0.0)]),
             (-1, ))))
    result_times.sort()
    assert ((result_times >= 0).all())
    assert ((result_times <= dt).all())

    for t in result_times:
        if on_before_off:
            U = numpy.matrix([[vcc], [vcc], [vcc]])
            U[t <= Ton] = 0.0
            U[Toff < t] = 0.0
        else:
            U = numpy.matrix([[0.0], [0.0], [0.0]])
            U[t > Ton] = vcc
            U[t <= Toff] = vcc
        result.append((float(t + start_time), U.copy()))

    return result


def sample_times(T, dt, n, start_time):
    if switching_pattern == 'rear':
        T = 1.0 - T
        ans = full_sample_times(T,
                                numpy.matrix(numpy.ones((3, 1))) * 1.0, dt, n,
                                start_time)
    elif switching_pattern == 'centered front shifted':
        # Centered, but shifted to the beginning of the cycle.
        Ton = 0.5 - T / 2.0
        Toff = 0.5 + T / 2.0

        tn = min(Ton)[0, 0]
        Ton -= tn
        Toff -= tn

        ans = full_sample_times(Ton, Toff, dt, n, start_time)
    elif switching_pattern == 'centered':
        # Centered, looks waaay better.
        Ton = 0.5 - T / 2.0
        Toff = 0.5 + T / 2.0

        ans = full_sample_times(Ton, Toff, dt, n, start_time)
    elif switching_pattern == 'anticentered':
        # Centered, looks waaay better.
        Toff = T / 2.0
        Ton = 1.0 - T / 2.0

        ans = full_sample_times(Ton, Toff, dt, n, start_time)
    elif switching_pattern == 'front':
        ans = full_sample_times(numpy.matrix(numpy.zeros((3, 1))), T, dt, n,
                                start_time)
    else:
        assert (False)

    return ans


class DataLogger(object):

    def __init__(self, title=None):
        self.title = title
        self.ia = []
        self.ib = []
        self.ic = []
        self.ia_goal = []
        self.ib_goal = []
        self.ic_goal = []
        self.ia_controls = []
        self.ib_controls = []
        self.ic_controls = []
        self.isensea = []
        self.isenseb = []
        self.isensec = []

        self.va = []
        self.vb = []
        self.vc = []
        self.van = []
        self.vbn = []
        self.vcn = []

        self.ea = []
        self.eb = []
        self.ec = []

        self.theta = []
        self.omega = []

        self.i_goal = []

        self.time = []
        self.controls_time = []
        self.predicted_time = []

        self.ia_pred = []
        self.ib_pred = []
        self.ic_pred = []

        self.voltage_time = []
        self.estimated_velocity = []
        self.U_last = numpy.matrix(numpy.zeros((3, 1)))

    def log_predicted(self, current_time, p):
        self.predicted_time.append(current_time)
        self.ia_pred.append(p[0, 0])
        self.ib_pred.append(p[1, 0])
        self.ic_pred.append(p[2, 0])

    def log_controls(self, current_time, measured_current, In, E,
                     estimated_velocity):
        self.controls_time.append(current_time)
        self.ia_controls.append(measured_current[0, 0])
        self.ib_controls.append(measured_current[1, 0])
        self.ic_controls.append(measured_current[2, 0])

        self.ea.append(E[0, 0])
        self.eb.append(E[1, 0])
        self.ec.append(E[2, 0])

        self.ia_goal.append(In[0, 0])
        self.ib_goal.append(In[1, 0])
        self.ic_goal.append(In[2, 0])
        self.estimated_velocity.append(estimated_velocity)

    def log_data(self, X, U, current_time, Vn, i_goal):
        self.ia.append(X[0, 0])
        self.ib.append(X[1, 0])
        self.ic.append(X[2, 0])

        self.i_goal.append(i_goal)

        self.isensea.append(X[5, 0])
        self.isenseb.append(X[6, 0])
        self.isensec.append(X[7, 0])

        self.theta.append(X[3, 0])
        self.omega.append(X[4, 0])

        self.time.append(current_time)

        self.van.append(Vn[0, 0])
        self.vbn.append(Vn[1, 0])
        self.vcn.append(Vn[2, 0])

        if (self.U_last != U).any():
            self.va.append(self.U_last[0, 0])
            self.vb.append(self.U_last[1, 0])
            self.vc.append(self.U_last[2, 0])
            self.voltage_time.append(current_time)

            self.va.append(U[0, 0])
            self.vb.append(U[1, 0])
            self.vc.append(U[2, 0])
            self.voltage_time.append(current_time)
            self.U_last = U.copy()

    def plot(self):
        fig = pylab.figure()
        pylab.subplot(3, 1, 1)
        pylab.plot(self.controls_time,
                   self.ia_controls,
                   'ro',
                   label='ia_controls')
        pylab.plot(self.controls_time,
                   self.ib_controls,
                   'go',
                   label='ib_controls')
        pylab.plot(self.controls_time,
                   self.ic_controls,
                   'bo',
                   label='ic_controls')
        pylab.plot(self.controls_time, self.ia_goal, 'r--', label='ia_goal')
        pylab.plot(self.controls_time, self.ib_goal, 'g--', label='ib_goal')
        pylab.plot(self.controls_time, self.ic_goal, 'b--', label='ic_goal')

        #pylab.plot(self.controls_time, self.ia_pred, 'r*', label='ia_pred')
        #pylab.plot(self.controls_time, self.ib_pred, 'g*', label='ib_pred')
        #pylab.plot(self.controls_time, self.ic_pred, 'b*', label='ic_pred')
        pylab.plot(self.time, self.isensea, 'r:', label='ia_sense')
        pylab.plot(self.time, self.isenseb, 'g:', label='ib_sense')
        pylab.plot(self.time, self.isensec, 'b:', label='ic_sense')
        pylab.plot(self.time, self.ia, 'r', label='ia')
        pylab.plot(self.time, self.ib, 'g', label='ib')
        pylab.plot(self.time, self.ic, 'b', label='ic')
        pylab.plot(self.time, self.i_goal, label='i_goal')
        if self.title is not None:
            fig.canvas.set_window_title(self.title)
        pylab.legend()

        pylab.subplot(3, 1, 2)
        pylab.plot(self.voltage_time, self.va, label='va')
        pylab.plot(self.voltage_time, self.vb, label='vb')
        pylab.plot(self.voltage_time, self.vc, label='vc')
        pylab.plot(self.time, self.van, label='van')
        pylab.plot(self.time, self.vbn, label='vbn')
        pylab.plot(self.time, self.vcn, label='vcn')
        pylab.plot(self.controls_time, self.ea, label='ea')
        pylab.plot(self.controls_time, self.eb, label='eb')
        pylab.plot(self.controls_time, self.ec, label='ec')
        pylab.legend()

        pylab.subplot(3, 1, 3)
        pylab.plot(self.time, self.theta, label='theta')
        pylab.plot(self.time, self.omega, label='omega')
        #pylab.plot(self.controls_time, self.estimated_velocity, label='estimated omega')

        pylab.legend()

        fig = pylab.figure()
        pylab.plot(self.controls_time,
                   map(operator.sub, self.ia_goal, self.ia_controls),
                   'r',
                   label='ia_error')
        pylab.plot(self.controls_time,
                   map(operator.sub, self.ib_goal, self.ib_controls),
                   'g',
                   label='ib_error')
        pylab.plot(self.controls_time,
                   map(operator.sub, self.ic_goal, self.ic_controls),
                   'b',
                   label='ic_error')
        if self.title is not None:
            fig.canvas.set_window_title(self.title)
        pylab.legend()
        pylab.show()


# So, from running a bunch of math, we know the following:
# Van + Vbn + Vcn = 0
# ia + ib + ic = 0
# ea + eb + ec = 0
# d ia/dt + d ib/dt + d ic/dt = 0
#
# We also have:
#  [ Van ]   [  2/3 -1/3 -1/3] [Va]
#  [ Vbn ] = [ -1/3  2/3 -1/3] [Vb]
#  [ Vcn ]   [ -1/3 -1/3  2/3] [Vc]
#
# or,
#
#  Vabcn = Vconv * V
#
# The base equation is:
#
# [ Van ]   [ R 0 0 ] [ ia ]   [ L M M ] [ dia/dt ]   [ ea ]
# [ Vbn ] = [ 0 R 0 ] [ ib ] + [ M L M ] [ dib/dt ] + [ eb ]
# [ Vbn ]   [ 0 0 R ] [ ic ]   [ M M L ] [ dic/dt ]   [ ec ]
#
# or
#
# Vabcn = R_matrix * I + L_matrix * I_dot + E
#
# We can re-arrange this as:
#
# inv(L_matrix) * (Vconv * V - E - R_matrix * I) = I_dot
# B * V - inv(L_matrix) * E - A * I = I_dot
class Simulation(object):

    def __init__(self):
        self.R_matrix = numpy.matrix(numpy.eye(3)) * R
        self.L_matrix = numpy.matrix([[L, M, M], [M, L, M], [M, M, L]])
        self.L_matrix_inv = numpy.linalg.inv(self.L_matrix)
        self.A = self.L_matrix_inv * self.R_matrix
        self.B = self.L_matrix_inv * Vconv
        self.A_discrete, self.B_discrete = controls.c2d(
            -self.A, self.B, 1.0 / hz)
        self.B_discrete_inverse = numpy.matrix(
            numpy.eye(3)) / (self.B_discrete[0, 0] - self.B_discrete[1, 0])

        self.R_model = R * 1.0
        self.L_model = L * 1.0
        self.M_model = M * 1.0
        self.R_matrix_model = numpy.matrix(numpy.eye(3)) * self.R_model
        self.L_matrix_model = numpy.matrix(
            [[self.L_model, self.M_model, self.M_model],
             [self.M_model, self.L_model, self.M_model],
             [self.M_model, self.M_model, self.L_model]])
        self.L_matrix_inv_model = numpy.linalg.inv(self.L_matrix_model)
        self.A_model = self.L_matrix_inv_model * self.R_matrix_model
        self.B_model = self.L_matrix_inv_model * Vconv
        self.A_discrete_model, self.B_discrete_model = \
            controls.c2d(-self.A_model, self.B_model, 1.0 / hz)
        self.B_discrete_inverse_model = numpy.matrix(numpy.eye(3)) / (
            self.B_discrete_model[0, 0] - self.B_discrete_model[1, 0])

        print 'constexpr double kL = %g;' % self.L_model
        print 'constexpr double kM = %g;' % self.M_model
        print 'constexpr double kR = %g;' % self.R_model
        print 'constexpr float kAdiscrete_diagonal = %gf;' % self.A_discrete_model[
            0, 0]
        print 'constexpr float kAdiscrete_offdiagonal = %gf;' % self.A_discrete_model[
            1, 0]
        print 'constexpr float kBdiscrete_inv_diagonal = %gf;' % self.B_discrete_inverse_model[
            0, 0]
        print 'constexpr float kBdiscrete_inv_offdiagonal = %gf;' % self.B_discrete_inverse_model[
            1, 0]
        print 'constexpr double kOneAmpScalar = %g;' % one_amp_scalar
        print 'constexpr double kMaxOneAmpDrivingVoltage = %g;' % max_one_amp_driving_voltage
        print('A_discrete', self.A_discrete)
        print('B_discrete', self.B_discrete)
        print('B_discrete_sub', numpy.linalg.inv(self.B_discrete[0:2, 0:2]))
        print('B_discrete_inv', self.B_discrete_inverse)

        # Xdot[5:, :] = (R_sense2 + R_sense1) / R_sense2 * (
        #      (1.0 / (R_sense1 * C_sense)) * (-Isense * R_sense2 / (R_sense1 + R_sense2) * (R_sense1 / R_sense2 + 1.0) + I))
        self.mk1 = (R_sense2 + R_sense1) / R_sense2 * (1.0 /
                                                       (R_sense1 * C_sense))
        self.mk2 = -self.mk1 * R_sense2 / (R_sense1 + R_sense2) * (
            R_sense1 / R_sense2 + 1.0)

        # ia, ib, ic, theta, omega, isensea, isenseb, isensec
        self.X = numpy.matrix([[0.0], [0.0], [0.0], [-2.0 * numpy.pi / 3.0],
                               [0.0], [0.0], [0.0], [0.0]])

        self.K = 0.05 * Vconv
        print('A %s' % repr(self.A))
        print('B %s' % repr(self.B))
        print('K %s' % repr(self.K))

        print('System poles are %s' % repr(numpy.linalg.eig(self.A)[0]))
        print('Poles are %s' %
              repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))

        controllability = controls.ctrb(self.A, self.B)
        print('Rank of augmented controlability matrix. %d' %
              numpy.linalg.matrix_rank(controllability))

        self.data_logger = DataLogger(switching_pattern)
        self.current_time = 0.0

        self.estimated_velocity = self.X[4, 0]

    def motor_diffeq(self, x, t, U):
        I = numpy.matrix(x[0:3]).T
        theta = x[3]
        omega = x[4]
        Isense = numpy.matrix(x[5:]).T

        dflux = phases(f_single, theta) / Kv

        Xdot = numpy.matrix(numpy.zeros((8, 1)))
        di_dt = -self.A_model * I + self.B_model * U - self.L_matrix_inv_model * dflux * omega
        torque = I.T * dflux
        Xdot[0:3, :] = di_dt
        Xdot[3, :] = omega
        Xdot[4, :] = torque / J

        Xdot[5:, :] = self.mk1 * I + self.mk2 * Isense
        return numpy.squeeze(numpy.asarray(Xdot))

    def DoControls(self, goal_current):
        theta = self.X[3, 0]
        # Use the actual angular velocity.
        omega = self.X[4, 0]

        measured_current = self.X[5:, :].copy()

        # Ok, lets now fake it.
        E_imag1 = numpy.exp(1j * theta) * K1 * numpy.matrix(
            [[-1j], [-1j * numpy.exp(1j * numpy.pi * 2.0 / 3.0)],
             [-1j * numpy.exp(-1j * numpy.pi * 2.0 / 3.0)]])
        E_imag2 = numpy.exp(1j * 5.0 * theta) * K2 * numpy.matrix(
            [[-1j], [-1j * numpy.exp(-1j * numpy.pi * 2.0 / 3.0)],
             [-1j * numpy.exp(1j * numpy.pi * 2.0 / 3.0)]])

        overall_measured_current = ((E_imag1 + E_imag2).real.T *
                                    measured_current / one_amp_scalar)[0, 0]

        current_error = goal_current - overall_measured_current
        #print(current_error)
        self.estimated_velocity += current_error * 1.0
        omega = self.estimated_velocity

        # Now, apply the transfer function of the inductor.
        # Use that to difference the current across the cycle.
        Icurrent = self.Ilast
        # No history:
        #Icurrent = phases(g_single, theta) * goal_current
        Inext = phases(g_single, theta + omega * 1.0 / hz) * goal_current

        deltaI = Inext - Icurrent

        H1 = -numpy.linalg.inv(1j * omega * self.L_matrix +
                               self.R_matrix) * omega / Kv
        H2 = -numpy.linalg.inv(1j * omega * 5.0 * self.L_matrix +
                               self.R_matrix) * omega / Kv
        p_imag = H1 * E_imag1 + H2 * E_imag2
        p_next_imag = numpy.exp(1j * omega * 1.0 / hz) * H1 * E_imag1 + \
            numpy.exp(1j * omega * 5.0 * 1.0 / hz) * H2 * E_imag2
        p = p_imag.real

        # So, we now know how much the change in current is due to changes in BEMF.
        # Subtract that, and then run the stock statespace equation.
        Vn_ff = self.B_discrete_inverse * (Inext - self.A_discrete *
                                           (Icurrent - p) - p_next_imag.real)
        print 'Vn_ff', Vn_ff
        print 'Inext', Inext
        Vn = Vn_ff + self.K * (Icurrent - measured_current)

        E = phases(f_single, self.X[3, 0]) / Kv * self.X[4, 0]
        self.data_logger.log_controls(self.current_time, measured_current,
                                      Icurrent, E, self.estimated_velocity)

        self.Ilast = Inext

        return Vn

    def Simulate(self):
        start_wall_time = time.time()
        self.Ilast = numpy.matrix(numpy.zeros((3, 1)))
        for n in range(200):
            goal_current = 1.0
            max_current = (
                vcc - (self.X[4, 0] / Kv * 2.0)) / max_one_amp_driving_voltage
            min_current = (
                -vcc - (self.X[4, 0] / Kv * 2.0)) / max_one_amp_driving_voltage
            goal_current = max(min_current, min(max_current, goal_current))

            Vn = self.DoControls(goal_current)

            #Vn = numpy.matrix([[1.00], [0.0], [0.0]])
            Vn = numpy.matrix([[0.00], [1.00], [0.0]])
            #Vn = numpy.matrix([[0.00], [0.0], [1.00]])

            # T is the fractional rate.
            T = Vn / vcc
            tn = -numpy.min(T)
            T += tn
            if (T > 1.0).any():
                T = T / numpy.max(T)

            for t, U in sample_times(T=T,
                                     dt=1.0 / hz,
                                     n=10,
                                     start_time=self.current_time):
                # Analog amplifier mode!
                #U = Vn

                self.data_logger.log_data(self.X, (U - min(U)),
                                          self.current_time, Vn, goal_current)
                t_array = numpy.array([self.current_time, t])
                self.X = numpy.matrix(
                    scipy.integrate.odeint(self.motor_diffeq,
                                           numpy.squeeze(numpy.asarray(
                                               self.X)),
                                           t_array,
                                           args=(U, )))[1, :].T

                self.current_time = t

        print 'Took %f to simulate' % (time.time() - start_wall_time)

        self.data_logger.plot()


simulation = Simulation()
simulation.Simulate()