diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index 0066b93..699eb5a 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -247,6 +247,21 @@
 )
 
 py_binary(
+    name = "flywheel",
+    srcs = [
+        "flywheel.py",
+    ],
+    legacy_create_init = False,
+    target_compatible_with = ["@platforms//cpu:x86_64"],
+    deps = [
+        ":python_init",
+        "//frc971/control_loops/python:controls",
+        "@pip//matplotlib",
+        "@pip//pygobject",
+    ],
+)
+
+py_binary(
     name = "static_zeroing_single_dof_profiled_subsystem_test",
     srcs = [
         "static_zeroing_single_dof_profiled_subsystem_test.py",
diff --git a/frc971/control_loops/python/flywheel.py b/frc971/control_loops/python/flywheel.py
new file mode 100644
index 0000000..1280b36
--- /dev/null
+++ b/frc971/control_loops/python/flywheel.py
@@ -0,0 +1,329 @@
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+import numpy
+
+import matplotlib
+import matplotlib.pyplot as plt
+
+import glog
+
+
+class FlywheelParams(object):
+
+    def __init__(self,
+                 name,
+                 motor,
+                 G,
+                 J,
+                 q_pos,
+                 q_vel,
+                 q_voltage,
+                 r_pos,
+                 controller_poles,
+                 dt=0.00505):
+        self.name = name
+        self.motor = motor
+        self.G = G
+        self.J = J
+        self.q_pos = q_pos
+        self.q_vel = q_vel
+        self.q_voltage = q_voltage
+        self.r_pos = r_pos
+        self.dt = dt
+        self.controller_poles = controller_poles
+
+
+class VelocityFlywheel(control_loop.HybridControlLoop):
+
+    def __init__(self, params, name="Flywheel"):
+        super(VelocityFlywheel, self).__init__(name=name)
+        self.params = params
+        # Set Motor
+        self.motor = self.params.motor
+        # Moment of inertia of the flywheel wheel in kg m^2
+        self.J = self.params.J
+        # Gear ratio
+        self.G = self.params.G
+        # Control loop time step
+        self.dt = self.params.dt
+
+        # State feedback matrices
+        # [angular velocity]
+        self.A_continuous = numpy.matrix([[
+            -self.motor.Kt / self.motor.Kv /
+            (self.J * self.G * self.G * self.motor.resistance)
+        ]])
+        self.B_continuous = numpy.matrix(
+            [[self.motor.Kt / (self.J * self.G * self.motor.resistance)]])
+        self.C = numpy.matrix([[1]])
+        self.D = numpy.matrix([[0]])
+
+        self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+                                                   self.B_continuous, self.dt)
+
+        self.PlaceControllerPoles(self.params.controller_poles)
+
+        # Generated controller not used.
+        self.PlaceObserverPoles([0.3])
+
+        self.U_max = numpy.matrix([[12.0]])
+        self.U_min = numpy.matrix([[-12.0]])
+
+        qff_vel = 8.0
+        self.Qff = numpy.matrix([[1.0 / (qff_vel**2.0)]])
+
+        self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+
+        glog.debug('K: %s', str(self.K))
+        glog.debug('Poles: %s',
+                   str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+
+
+class Flywheel(VelocityFlywheel):
+
+    def __init__(self, params, name="Flywheel"):
+        super(Flywheel, self).__init__(params, name=name)
+
+        self.A_continuous_unaugmented = self.A_continuous
+        self.B_continuous_unaugmented = self.B_continuous
+
+        self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
+        self.A_continuous[1:2, 1:2] = self.A_continuous_unaugmented
+        self.A_continuous[0, 1] = 1
+
+        self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
+        self.B_continuous[1:2, 0] = self.B_continuous_unaugmented
+
+        # State feedback matrices
+        # [position, angular velocity]
+        self.C = numpy.matrix([[1, 0]])
+        self.D = numpy.matrix([[0]])
+
+        self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+                                                   self.B_continuous, self.dt)
+
+        rpl = 0.45
+        ipl = 0.07
+        self.PlaceObserverPoles([rpl + 1j * ipl, rpl - 1j * ipl])
+
+        self.K_unaugmented = self.K
+        self.K = numpy.matrix(numpy.zeros((1, 2)))
+        self.K[0, 1:2] = self.K_unaugmented
+        self.Kff_unaugmented = self.Kff
+        self.Kff = numpy.matrix(numpy.zeros((1, 2)))
+        self.Kff[0, 1:2] = self.Kff_unaugmented
+
+        self.InitializeState()
+
+
+class IntegralFlywheel(Flywheel):
+
+    def __init__(self, params, name="IntegralFlywheel"):
+        super(IntegralFlywheel, self).__init__(params, name=name)
+
+        self.A_continuous_unaugmented = self.A_continuous
+        self.B_continuous_unaugmented = self.B_continuous
+
+        self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+        self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+        self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
+
+        self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+        self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+
+        # states
+        # [position, velocity, voltage_error]
+        self.C_unaugmented = self.C
+        self.C = numpy.matrix(numpy.zeros((1, 3)))
+        self.C[0:1, 0:2] = self.C_unaugmented
+
+        glog.debug('A_continuous %s' % str(self.A_continuous))
+        glog.debug('B_continuous %s' % str(self.B_continuous))
+        glog.debug('C %s' % str(self.C))
+
+        self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+                                                   self.B_continuous, self.dt)
+
+        glog.debug('A %s' % str(self.A))
+        glog.debug('B %s' % str(self.B))
+
+        q_pos = self.params.q_pos
+        q_vel = self.params.q_vel
+        q_voltage = self.params.q_voltage
+        self.Q_continuous = numpy.matrix([[(q_pos**2.0), 0.0, 0.0],
+                                          [0.0, (q_vel**2.0), 0.0],
+                                          [0.0, 0.0, (q_voltage**2.0)]])
+
+        r_pos = self.params.r_pos
+        self.R_continuous = numpy.matrix([[(r_pos**2.0)]])
+
+        _, _, self.Q, self.R = controls.kalmd(A_continuous=self.A_continuous,
+                                              B_continuous=self.B_continuous,
+                                              Q_continuous=self.Q_continuous,
+                                              R_continuous=self.R_continuous,
+                                              dt=self.dt)
+
+        glog.debug('Q_discrete %s' % (str(self.Q)))
+        glog.debug('R_discrete %s' % (str(self.R)))
+
+        self.KalmanGain, self.P_steady_state = controls.kalman(A=self.A,
+                                                               B=self.B,
+                                                               C=self.C,
+                                                               Q=self.Q,
+                                                               R=self.R)
+        self.L = self.A * self.KalmanGain
+
+        self.K_unaugmented = self.K
+        self.K = numpy.matrix(numpy.zeros((1, 3)))
+        self.K[0, 0:2] = self.K_unaugmented
+        self.K[0, 2] = 1
+        self.Kff_unaugmented = self.Kff
+        self.Kff = numpy.matrix(numpy.zeros((1, 3)))
+        self.Kff[0, 0:2] = self.Kff_unaugmented
+
+        self.InitializeState()
+
+
+def PlotSpinup(params, goal, iterations=400):
+    """Runs the flywheel plant with an initial condition and goal.
+
+    Args:
+        flywheel: Flywheel object to use.
+        goal: goal state.
+        iterations: Number of timesteps to run the model for.
+        controller_flywheel: Flywheel object to get K from, or None if we should
+             use flywheel.
+        observer_flywheel: Flywheel object to use for the observer, or None if we
+            should use the actual state.
+    """
+
+    # Various lists for graphing things.
+    t = []
+    x = []
+    v = []
+    a = []
+    x_hat = []
+    u = []
+    offset = []
+
+    flywheel = Flywheel(params, params.name)
+    controller_flywheel = IntegralFlywheel(params, params.name)
+    observer_flywheel = IntegralFlywheel(params, params.name)
+    vbat = 12.0
+
+    if t:
+        initial_t = t[-1] + flywheel.dt
+    else:
+        initial_t = 0
+
+    for i in range(iterations):
+        X_hat = flywheel.X
+
+        if observer_flywheel is not None:
+            X_hat = observer_flywheel.X_hat
+            x_hat.append(observer_flywheel.X_hat[1, 0])
+
+        ff_U = controller_flywheel.Kff * (goal - observer_flywheel.A * goal)
+
+        U = controller_flywheel.K * (goal - X_hat) + ff_U
+        U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+        x.append(flywheel.X[0, 0])
+
+        if v:
+            last_v = v[-1]
+        else:
+            last_v = 0
+
+        v.append(flywheel.X[1, 0])
+        a.append((v[-1] - last_v) / flywheel.dt)
+
+        if observer_flywheel is not None:
+            observer_flywheel.Y = flywheel.Y
+            observer_flywheel.CorrectHybridObserver(U)
+            offset.append(observer_flywheel.X_hat[2, 0])
+
+        applied_U = U.copy()
+        if i > 200:
+            applied_U += 2
+        flywheel.Update(applied_U)
+
+        if observer_flywheel is not None:
+            observer_flywheel.PredictHybridObserver(U, flywheel.dt)
+
+        t.append(initial_t + i * flywheel.dt)
+        u.append(U[0, 0])
+
+    matplotlib.use("GTK3Agg")
+
+    plt.subplot(3, 1, 1)
+    plt.plot(t, v, label='x')
+    plt.plot(t, x_hat, label='x_hat')
+    plt.legend()
+
+    plt.subplot(3, 1, 2)
+    plt.plot(t, u, label='u')
+    plt.plot(t, offset, label='voltage_offset')
+    plt.legend()
+
+    plt.subplot(3, 1, 3)
+    plt.plot(t, a, label='a')
+    plt.legend()
+
+    plt.show()
+
+
+def WriteFlywheel(params, plant_files, controller_files, namespace):
+    """Writes out the constants for a flywheel to a file.
+
+    Args:
+      params: list of Flywheel Params, the
+        parameters defining the system.
+      plant_files: list of strings, the cc and h files for the plant.
+      controller_files: list of strings, the cc and h files for the integral
+        controller.
+      namespaces: list of strings, the namespace list to use.
+    """
+    # Write the generated constants out to a file.
+    flywheels = []
+    integral_flywheels = []
+
+    if type(params) is list:
+        name = params[0].name
+        for index, param in enumerate(params):
+            flywheels.append(Flywheel(param, name=param.name + str(index)))
+            integral_flywheels.append(
+                IntegralFlywheel(param,
+                                 name='Integral' + param.name + str(index)))
+    else:
+        name = params.name
+        flywheels.append(Flywheel(params, params.name))
+        integral_flywheels.append(
+            IntegralFlywheel(params, name='Integral' + params.name))
+
+    loop_writer = control_loop.ControlLoopWriter(name,
+                                                 flywheels,
+                                                 namespaces=namespace)
+    loop_writer.AddConstant(
+        control_loop.Constant('kOutputRatio', '%f', flywheels[0].G))
+    loop_writer.AddConstant(
+        control_loop.Constant('kFreeSpeed', '%f',
+                              flywheels[0].motor.free_speed))
+    loop_writer.AddConstant(
+        control_loop.Constant('kBemf',
+                              '%f',
+                              flywheels[0].motor.Kv * flywheels[0].G,
+                              comment="// Radians/sec / volt"))
+    loop_writer.AddConstant(
+        control_loop.Constant('kResistance',
+                              '%f',
+                              flywheels[0].motor.resistance,
+                              comment="// Ohms"))
+    loop_writer.Write(plant_files[0], plant_files[1])
+
+    integral_loop_writer = control_loop.ControlLoopWriter(
+        'Integral' + name,
+        integral_flywheels,
+        namespaces=namespace,
+        plant_type='StateFeedbackHybridPlant',
+        observer_type='HybridKalman')
+    integral_loop_writer.Write(controller_files[0], controller_files[1])