diff --git a/frc971/codelab/BUILD b/frc971/codelab/BUILD
new file mode 100644
index 0000000..2823200
--- /dev/null
+++ b/frc971/codelab/BUILD
@@ -0,0 +1,39 @@
+package(default_visibility = ['//visibility:public'])
+
+load('//aos/build:queues.bzl', 'queue_library')
+
+cc_binary(
+  name = 'basic_test',
+  srcs = ['basic_test.cc'],
+  deps = [
+    ':basic_queue',
+    ':basic',
+    '//aos/testing:googletest',
+    '//aos/common:queues',
+    '//aos/common/controls:control_loop_test',
+    '//frc971/control_loops:state_feedback_loop',
+    '//frc971/control_loops:team_number_test_environment',
+  ],
+  testonly = 1,
+)
+
+cc_library(
+  name = 'basic',
+  srcs = ['basic.cc'],
+  hdrs = ['basic.h'],
+  deps = [
+    ':basic_queue',
+    '//aos/common/controls:control_loop',
+  ],
+)
+
+queue_library(
+  name = 'basic_queue',
+  srcs = [
+    'basic.q',
+  ],
+  deps = [
+    '//aos/common/controls:control_loop_queues',
+    '//frc971/control_loops:queues',
+  ],
+)
diff --git a/frc971/codelab/basic.cc b/frc971/codelab/basic.cc
new file mode 100644
index 0000000..536384c
--- /dev/null
+++ b/frc971/codelab/basic.cc
@@ -0,0 +1,22 @@
+#include "frc971/codelab/basic.h"
+
+namespace frc971 {
+namespace codelab {
+
+Basic::Basic(BasicQueue *my_basic_queue)
+    : aos::controls::ControlLoop<BasicQueue>(my_basic_queue) {}
+
+void Basic::RunIteration(const BasicQueue::Goal *goal,
+                         const BasicQueue::Position *position,
+                         BasicQueue::Output *output,
+                         BasicQueue::Status *status) {
+  // TODO(you): Set the intake_voltage to 12 Volts when
+  // intake is requested (via intake in goal). Make sure not to set
+  // the motor to anything but 0 V when the limit_sensor is pressed.
+
+  // Ignore: These are to avoid clang warnings.
+  (void)goal, (void)position, (void)output, (void)status;
+}
+
+}  // namespace codelab
+}  // namespace frc971
diff --git a/frc971/codelab/basic.h b/frc971/codelab/basic.h
new file mode 100644
index 0000000..b67fdbd
--- /dev/null
+++ b/frc971/codelab/basic.h
@@ -0,0 +1,58 @@
+#ifndef FRC971_CODELAB_BASIC_H_
+#define FRC971_CODELAB_BASIC_H_
+
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/time.h"
+
+#include "frc971/codelab/basic.q.h"
+
+namespace frc971 {
+namespace codelab {
+
+// This codelab helps build basic knowledge of how to use 971 control loop
+// primatives.
+//
+// The meat of the task is to make the tests pass.
+//
+// Run the tests with:
+//  $ bazel run //frc971/codelab:basic_test -- --gtest_color=yes
+//
+// Control loops all follow the same convention:
+//  There are 4 queues (goal, position, status, output).
+//
+//  2 queues are input queues: goal, position.
+//  2 queues are output queues: output, status.
+//
+// ::aos::controls::ControlLoop is a helper class that takes
+// a queue_group type from a .h file, and organizes to call
+// RunIteration() at a consistent interval. It will fetch from
+// goal and position messages from the goal and position queue,
+// and publish an output and status result to the output and status
+// queues.
+//
+// The basic.q file will construct boilerplate c++ code for the
+// Goal, Position, Status, Message
+// types, and construct static variables for fetching these named queues.
+// Inquisitive souls can check out:
+//  $ bazel build //frc971/codelab:basic_queue
+//  $ vim bazel-genfiles/frc971/codelab/basic.q.{cc,h} -o
+//  from the 971-Robot-Code directory.
+//
+// Order of approaching this should be:
+// - Read the BUILD file and learn about what code is being generated.
+// - Read basic.q, and familiarize yourself on the inputs and types involved.
+class Basic : public ::aos::controls::ControlLoop<BasicQueue> {
+ public:
+  explicit Basic(BasicQueue *my_basic_queue = &basic_queue);
+
+ protected:
+  void RunIteration(const BasicQueue::Goal *goal,
+                    const BasicQueue::Position *position,
+                    BasicQueue::Output *output,
+                    BasicQueue::Status *status) override;
+};
+
+}  // namespace codelab
+}  // namespace frc971
+
+#endif  // FRC971_CODELAB_BASIC_H_
diff --git a/frc971/codelab/basic.q b/frc971/codelab/basic.q
new file mode 100644
index 0000000..8183271
--- /dev/null
+++ b/frc971/codelab/basic.q
@@ -0,0 +1,44 @@
+package frc971.codelab;
+
+import "aos/common/controls/control_loops.q";
+import "frc971/control_loops/control_loops.q";
+
+// The theme of this basic test is a simple intake system.
+//
+// The system will have a motor driven by the voltage returned
+// by output, and then eventually this motor, when run enough,
+// will trigger the limit_sensor. The hypothetical motor should shut
+// off in that hypothetical situation to avoid hypothetical burnout.
+queue_group BasicQueue {
+  implements aos.control_loops.ControlLoop;
+
+  message Goal {
+    // The control loop needs to intake now.
+    bool intake;
+  };
+
+  message Position {
+    // This is a potential incoming sensor value letting us know
+    // if we need to be intaking.
+    bool limit_sensor;
+  };
+
+  message Status {
+    // Lets consumers of basic_queue.status know if
+    // the requested intake is finished.
+    bool intake_complete;
+  };
+
+  message Output {
+    // This would be set up to drive a hypothetical motor that would
+    // hope to intake something.
+    double intake_voltage;
+  };
+
+  queue Goal goal;
+  queue Position position;
+  queue Output output;
+  queue Status status;
+};
+
+queue_group BasicQueue basic_queue;
diff --git a/frc971/codelab/basic_test.cc b/frc971/codelab/basic_test.cc
new file mode 100644
index 0000000..0acf9e7
--- /dev/null
+++ b/frc971/codelab/basic_test.cc
@@ -0,0 +1,170 @@
+#include "frc971/codelab/basic.h"
+
+#include <unistd.h>
+
+#include <chrono>
+#include <memory>
+
+#include "aos/common/controls/control_loop_test.h"
+#include "aos/common/queue.h"
+#include "frc971/codelab/basic.q.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace codelab {
+namespace testing {
+
+// Class which simulates stuff and sends out queue messages with the
+// position.
+class BasicSimulation {
+ public:
+  BasicSimulation()
+      : basic_queue_(".frc971.codelab.basic_queue", 0x78d8e372,
+                     ".frc971.codelab.basic_queue.goal",
+                     ".frc971.codelab.basic_queue.position",
+                     ".frc971.codelab.basic_queue.output",
+                     ".frc971.codelab.basic_queue.status") {}
+
+  // Sends a queue message with the position data.
+  void SendPositionMessage() {
+    ::aos::ScopedMessagePtr<BasicQueue::Position> position =
+        basic_queue_.position.MakeMessage();
+
+    position->limit_sensor = limit_sensor_;
+
+    position.Send();
+  }
+
+  void VerifyResults(double voltage, bool status) {
+    basic_queue_.output.FetchLatest();
+    basic_queue_.status.FetchLatest();
+
+    ASSERT_TRUE(basic_queue_.output.get() != nullptr);
+    ASSERT_TRUE(basic_queue_.status.get() != nullptr);
+
+    EXPECT_EQ(basic_queue_.output->intake_voltage, voltage);
+    EXPECT_EQ(basic_queue_.status->intake_complete, status);
+  }
+
+  void set_limit_sensor(bool value) { limit_sensor_ = value; }
+
+  // Simulates basic control loop for a single timestep.
+  void Simulate() { EXPECT_TRUE(basic_queue_.output.FetchLatest()); }
+
+ private:
+  BasicQueue basic_queue_;
+  bool limit_sensor_ = false;
+};
+
+class BasicControlLoopTest : public ::aos::testing::ControlLoopTest {
+ public:
+  BasicControlLoopTest()
+      : basic_queue_(".frc971.codelab.basic_queue", 0x78d8e372,
+                     ".frc971.codelab.basic_queue.goal",
+                     ".frc971.codelab.basic_queue.position",
+                     ".frc971.codelab.basic_queue.output",
+                     ".frc971.codelab.basic_queue.status"),
+        basic_loop_(&basic_queue_) {
+    set_team_id(control_loops::testing::kTeamNumber);
+  }
+
+  // Runs one iteration of the whole simulation.
+  void RunIteration(bool enabled = true) {
+    SendMessages(enabled);
+
+    basic_simulation_.SendPositionMessage();
+    basic_loop_.Iterate();
+    basic_simulation_.Simulate();
+
+    TickTime();
+  }
+
+  BasicQueue basic_queue_;
+  Basic basic_loop_;
+  BasicSimulation basic_simulation_;
+};
+
+// Tests that when the motor has finished intaking it stops.
+TEST_F(BasicControlLoopTest, IntakeLimitTransitionsToTrue) {
+  ASSERT_TRUE(basic_queue_.goal.MakeWithBuilder().intake(true).Send());
+  basic_simulation_.set_limit_sensor(false);
+
+  RunIteration();
+
+  basic_simulation_.VerifyResults(12.0, false);
+
+  ASSERT_TRUE(basic_queue_.goal.MakeWithBuilder().intake(true).Send());
+  basic_simulation_.set_limit_sensor(true);
+
+  RunIteration();
+
+  basic_simulation_.VerifyResults(0.0, true);
+}
+
+// Tests that the intake goes on if the sensor is not pressed
+// and intake is requested.
+TEST_F(BasicControlLoopTest, IntakeLimitNotSet) {
+  ASSERT_TRUE(basic_queue_.goal.MakeWithBuilder().intake(true).Send());
+  basic_simulation_.set_limit_sensor(false);
+
+  RunIteration();
+
+  basic_simulation_.VerifyResults(12.0, false);
+}
+
+// Tests that the intake is off if no intake is requested,
+// even if the limit sensor is off.
+TEST_F(BasicControlLoopTest, NoIntakeLimitNotSet) {
+  ASSERT_TRUE(basic_queue_.goal.MakeWithBuilder().intake(false).Send());
+  basic_simulation_.set_limit_sensor(false);
+
+  RunIteration();
+
+  basic_simulation_.VerifyResults(0.0, false);
+}
+
+// Tests that the intake is off even if the limit sensor
+// is pressed and intake is requested.
+TEST_F(BasicControlLoopTest, IntakeLimitSet) {
+  ASSERT_TRUE(basic_queue_.goal.MakeWithBuilder().intake(true).Send());
+  basic_simulation_.set_limit_sensor(true);
+
+  RunIteration();
+
+  basic_simulation_.VerifyResults(0.0, true);
+}
+
+// Tests that the intake is off if no intake is requested,
+TEST_F(BasicControlLoopTest, NoIntakeLimitSet) {
+  ASSERT_TRUE(basic_queue_.goal.MakeWithBuilder().intake(false).Send());
+  basic_simulation_.set_limit_sensor(true);
+
+  RunIteration();
+
+  basic_simulation_.VerifyResults(0.0, false);
+}
+
+// Tests that the control loop handles things properly if no goal is set.
+TEST_F(BasicControlLoopTest, NoGoalSet) {
+  basic_simulation_.set_limit_sensor(true);
+
+  RunIteration();
+
+  basic_simulation_.VerifyResults(0.0, false);
+}
+
+// Tests that the control loop handles things properly if disabled.
+TEST_F(BasicControlLoopTest, Disabled) {
+  basic_simulation_.set_limit_sensor(true);
+
+  ASSERT_TRUE(basic_queue_.goal.MakeWithBuilder().intake(true).Send());
+
+  RunIteration(false);
+
+  basic_simulation_.VerifyResults(0.0, false);
+}
+
+}  // namespace testing
+}  // namespace codelab
+}  // namespace frc971
diff --git a/motors/python/BUILD b/motors/python/BUILD
new file mode 100644
index 0000000..20dd786
--- /dev/null
+++ b/motors/python/BUILD
@@ -0,0 +1,12 @@
+py_binary(
+    name = "phase_current",
+    srcs = [
+        "phase_current.py",
+    ],
+    deps = [
+        "//external:python-gflags",
+        "//external:python-glog",
+        "//frc971/control_loops/python:controls",
+    ],
+    restricted_to = ["//tools:k8"],
+)
diff --git a/motors/python/phase_current.py b/motors/python/phase_current.py
new file mode 100755
index 0000000..2e88fae
--- /dev/null
+++ b/motors/python/phase_current.py
@@ -0,0 +1,563 @@
+#!/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 = -2.65e03
+
+# Make the amplitude of the fundamental 1 for ease of playing with.
+K2 /= K1
+K1 = 1
+
+vcc = 30.0  # volts
+R_motor = 0.0055  # ohms for the motor
+R = 0.008  # ohms for system
+
+L = 10.0 * 1e-6  # Henries
+M = L / 10.0
+
+Kv = 22000.0 * 2.0 * numpy.pi / 60.0 / vcc * 2.0
+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.
+
+  Returns:
+    array of [t, U matrix]
+  """
+
+  assert((Toff <= 1.0).all())
+
+  if (Ton <= Toff).all():
+    # Verify that they are all ordered correctly.
+    on_before_off = True
+  else:
+    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()
+
+  for t in numpy.nditer(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], [0.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 = 10.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([[0.20], [0.0], [0.0]])
+      #Vn = numpy.matrix([[0.00], [0.20], [0.0]])
+      #Vn = numpy.matrix([[0.00], [0.0], [0.20]])
+
+      # 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()