diff --git a/y2022/control_loops/python/BUILD b/y2022/control_loops/python/BUILD
index 286b874..17f7b2a 100644
--- a/y2022/control_loops/python/BUILD
+++ b/y2022/control_loops/python/BUILD
@@ -55,6 +55,8 @@
     ],
     target_compatible_with = ["@platforms//cpu:x86_64"],
     deps = [
+        "//aos/util:py_trapezoid_profile",
+        "//frc971/control_loops/python:angular_system",
         "//frc971/control_loops/python:controls",
         "@matplotlib_repo//:matplotlib3",
     ],
diff --git a/y2022/control_loops/python/catapult.py b/y2022/control_loops/python/catapult.py
index 491286e..ce7496a 100755
--- a/y2022/control_loops/python/catapult.py
+++ b/y2022/control_loops/python/catapult.py
@@ -40,27 +40,35 @@
 M_cup = (1750 * 0.0254 * 0.04 * 2 * math.pi * (ball_diameter / 2.)**2.0)
 J_cup = M_cup * lever**2.0 + M_cup * (ball_diameter / 2.)**2.0
 
-print("J ball", ball_mass * lever * lever)
-print("J bar", J_bar)
-print("bar mass", M_bar)
-print("J cup", J_cup)
-print("cup mass", M_cup)
 
 J = (J_ball + J_bar + J_cup * 1.5)
-print("J", J)
+JEmpty = (J_bar + J_cup * 1.5)
 
-kCatapult = catapult_lib.CatapultParams(
+kCatapultWithBall = catapult_lib.CatapultParams(
     name='Catapult',
     motor=AddResistance(control_loop.NMotor(control_loop.Falcon(), 2), 0.03),
     G=G,
     J=J,
-    lever=lever,
-    q_pos=0.01,
-    q_vel=10.0,
-    q_voltage=4.0,
-    r_pos=0.01,
-    controller_poles=[.93],
-    dt=0.00505)
+    radius=lever,
+    q_pos=2.8,
+    q_vel=20.0,
+    kalman_q_pos=0.12,
+    kalman_q_vel=1.0,
+    kalman_q_voltage=1.5,
+    kalman_r_position=0.05)
+
+kCatapultEmpty = catapult_lib.CatapultParams(
+    name='Catapult',
+    motor=AddResistance(control_loop.NMotor(control_loop.Falcon(), 2), 0.03),
+    G=G,
+    J=JEmpty,
+    radius=lever,
+    q_pos=2.8,
+    q_vel=20.0,
+    kalman_q_pos=0.12,
+    kalman_q_vel=1.0,
+    kalman_q_voltage=1.5,
+    kalman_r_position=0.05)
 
 # Ideas for adjusting the cost function:
 #
@@ -252,7 +260,7 @@
 
 
 def CatapultProblem():
-    c = catapult_lib.Catapult(kCatapult)
+    c = catapult_lib.Catapult(kCatapultWithBall)
 
     kHorizon = 40
 
@@ -300,54 +308,69 @@
 
 
 def main(argv):
-    # Do all our math with a lower voltage so we have headroom.
-    U = numpy.matrix([[9.0]])
-
-    prob = osqp.OSQP()
-
-    kHorizon = 40
-    catapult = catapult_lib.Catapult(kCatapult)
-    X_initial = numpy.matrix([[0.0], [0.0]])
-    X_final = numpy.matrix([[2.0], [25.0]])
-    P, q, c = quadratic_cost(catapult, X_initial, X_final, kHorizon)
-    A = numpy.identity(kHorizon)
-    l = numpy.zeros((kHorizon, 1))
-    u = numpy.ones((kHorizon, 1)) * 12.0
-
-    prob.setup(scipy.sparse.csr_matrix(P),
-               q,
-               scipy.sparse.csr_matrix(A),
-               l,
-               u,
-               warm_start=True)
-
-    result = prob.solve()
-    # Check solver status
-    if result.info.status != 'solved':
-        raise ValueError('OSQP did not solve the problem!')
-
-    # Apply first control input to the plant
-    print(result.x)
-
     if FLAGS.plot:
-        print(
+        # Do all our math with a lower voltage so we have headroom.
+        U = numpy.matrix([[9.0]])
+
+        prob = osqp.OSQP()
+
+        kHorizon = 40
+        catapult = catapult_lib.Catapult(kCatapultWithBall)
+        X_initial = numpy.matrix([[0.0], [0.0]])
+        X_final = numpy.matrix([[2.0], [25.0]])
+        P, q, c = quadratic_cost(catapult, X_initial, X_final, kHorizon)
+        A = numpy.identity(kHorizon)
+        l = numpy.zeros((kHorizon, 1))
+        u = numpy.ones((kHorizon, 1)) * 12.0
+
+        prob.setup(scipy.sparse.csr_matrix(P),
+                   q,
+                   scipy.sparse.csr_matrix(A),
+                   l,
+                   u,
+                   warm_start=True)
+
+        result = prob.solve()
+        # Check solver status
+        if result.info.status != 'solved':
+            raise ValueError('OSQP did not solve the problem!')
+
+        # Apply first control input to the plant
+        print(result.x)
+
+        glog.debug("J ball", ball_mass * lever * lever)
+        glog.debug("J bar", J_bar)
+        glog.debug("bar mass", M_bar)
+        glog.debug("J cup", J_cup)
+        glog.debug("cup mass", M_cup)
+        glog.debug("J", J)
+        glog.debug("J Empty", JEmpty)
+
+        glog.debug(
             "For G:", G, " max speed ",
-            catapult_lib.MaxSpeed(params=kCatapult,
+            catapult_lib.MaxSpeed(params=kCatapultWithBall,
                                   U=U,
                                   final_position=math.pi / 2.0))
 
         CatapultProblem()
+
+        catapult_lib.PlotStep(params=kCatapultWithBall,
+                              R=numpy.matrix([[1.0], [0.0]]))
+        catapult_lib.PlotKick(params=kCatapultWithBall,
+                              R=numpy.matrix([[1.0], [0.0]]))
         return 0
 
-        catapult_lib.PlotShot(kCatapult, U, final_position=math.pi / 4.0)
+        catapult_lib.PlotShot(kCatapultWithBall,
+                              U,
+                              final_position=math.pi / 4.0)
 
         gs = []
         speed = []
         for i in numpy.linspace(0.01, 0.15, 150):
-            kCatapult.G = i
-            gs.append(kCatapult.G)
+            kCatapultWithBall.G = i
+            gs.append(kCatapultWithBall.G)
             speed.append(
-                catapult_lib.MaxSpeed(params=kCatapult,
+                catapult_lib.MaxSpeed(params=kCatapultWithBall,
                                       U=U,
                                       final_position=math.pi / 2.0))
         pylab.plot(gs, speed, label="max_speed")
@@ -359,10 +382,11 @@
         )
     else:
         namespaces = ['y2022', 'control_loops', 'superstructure', 'catapult']
-        catapult_lib.WriteCatapult(kCatapult, argv[1:3], argv[3:5], namespaces)
+        catapult_lib.WriteCatapult([kCatapultWithBall, kCatapultEmpty], argv[1:3], argv[3:5], namespaces)
     return 0
 
 
 if __name__ == '__main__':
     argv = FLAGS(sys.argv)
+    glog.init()
     sys.exit(main(argv))
diff --git a/y2022/control_loops/python/catapult_lib.py b/y2022/control_loops/python/catapult_lib.py
index 2606c44..d6040d1 100644
--- a/y2022/control_loops/python/catapult_lib.py
+++ b/y2022/control_loops/python/catapult_lib.py
@@ -1,180 +1,33 @@
+from frc971.control_loops.python import angular_system
 from frc971.control_loops.python import control_loop
 from frc971.control_loops.python import controls
+from aos.util.trapezoid_profile import TrapezoidProfile
 import numpy
 from matplotlib import pylab
 
 import gflags
 import glog
 
-
-class CatapultParams(object):
-    def __init__(self,
-                 name,
-                 motor,
-                 G,
-                 J,
-                 lever,
-                 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.lever = lever
-        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
+CatapultParams = angular_system.AngularSystemParams
 
 
-class VelocityCatapult(control_loop.HybridControlLoop):
+# TODO(austin): This is mostly the same as angular_system.  Can we either wrap an angular_system or assign it?
+class Catapult(angular_system.AngularSystem):
     def __init__(self, params, name="Catapult"):
-        super(VelocityCatapult, self).__init__(name=name)
-        self.params = params
-        # Set Motor
-        self.motor = self.params.motor
-        # Gear ratio
-        self.G = self.params.G
-        # Moment of inertia of the catapult wheel in kg m^2
-        self.J = self.params.J + self.motor.motor_inertia / (self.G**2.0)
-        # 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 Catapult(VelocityCatapult):
-    def __init__(self, params, name="Catapult"):
-        super(Catapult, 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
+        super(Catapult, self).__init__(params, name)
+        # Signal that we have a single cycle output delay to compensate for in
+        # our observer.
+        self.delayed_u = True
 
         self.InitializeState()
 
 
-class IntegralCatapult(Catapult):
+class IntegralCatapult(angular_system.IntegralAngularSystem):
     def __init__(self, params, name="IntegralCatapult"):
         super(IntegralCatapult, 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
+        # Signal that we have a single cycle output delay to compensate for in
+        # our observer.
+        self.delayed_u = True
 
         self.InitializeState()
 
@@ -201,7 +54,7 @@
     while True:
         X_hat = catapult.X
         if catapult.X[0, 0] > final_position:
-            return catapult.X[1, 0] * params.lever
+            return catapult.X[1, 0] * params.radius
 
         if observer_catapult is not None:
             X_hat = observer_catapult.X_hat
@@ -210,13 +63,13 @@
 
         if observer_catapult is not None:
             observer_catapult.Y = catapult.Y
-            observer_catapult.CorrectHybridObserver(U)
+            observer_catapult.CorrectObserver(U)
 
         applied_U = U.copy()
         catapult.Update(applied_U)
 
         if observer_catapult is not None:
-            observer_catapult.PredictHybridObserver(U, catapult.dt)
+            observer_catapult.PredictObserver(U)
 
 
 def PlotShot(params, U, final_position):
@@ -262,7 +115,7 @@
             X_hat = observer_catapult.X_hat
             x_hat.append(observer_catapult.X_hat[0, 0])
             w_hat.append(observer_catapult.X_hat[1, 0])
-            v_hat.append(observer_catapult.X_hat[1, 0] * params.lever)
+            v_hat.append(observer_catapult.X_hat[1, 0] * params.radius)
 
         U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
         x.append(catapult.X[0, 0])
@@ -277,13 +130,13 @@
 
         if observer_catapult is not None:
             observer_catapult.Y = catapult.Y
-            observer_catapult.CorrectHybridObserver(U)
+            observer_catapult.CorrectObserver(U)
             offset.append(observer_catapult.X_hat[2, 0])
 
         catapult.Update(U)
 
         if observer_catapult is not None:
-            observer_catapult.PredictHybridObserver(U, catapult.dt)
+            observer_catapult.PredictObserver(U)
 
         t.append(initial_t + i * catapult.dt)
         u.append(U[0, 0])
@@ -307,6 +160,95 @@
 
     pylab.show()
 
+
+RunTest = angular_system.RunTest
+
+
+def PlotStep(params, R, plant_params=None):
+    """Plots a step move to the goal.
+
+    Args:
+      params: CatapultParams for the controller and observer
+      plant_params: CatapultParams for the plant.  Defaults to params if
+        plant_params is None.
+      R: numpy.matrix(2, 1), the goal"""
+    plant = Catapult(plant_params or params, params.name)
+    controller = IntegralCatapult(params, params.name)
+    observer = IntegralCatapult(params, params.name)
+
+    # Test moving the system.
+    initial_X = numpy.matrix([[0.0], [0.0]])
+    augmented_R = numpy.matrix(numpy.zeros((3, 1)))
+    augmented_R[0:2, :] = R
+    RunTest(plant,
+            end_goal=augmented_R,
+            controller=controller,
+            observer=observer,
+            duration=2.0,
+            use_profile=False,
+            kick_time=1.0,
+            kick_magnitude=0.0)
+
+
+def PlotKick(params, R, plant_params=None):
+    """Plots a step motion with a kick at 1.0 seconds.
+
+    Args:
+      params: CatapultParams for the controller and observer
+      plant_params: CatapultParams for the plant.  Defaults to params if
+        plant_params is None.
+      R: numpy.matrix(2, 1), the goal"""
+    plant = Catapult(plant_params or params, params.name)
+    controller = IntegralCatapult(params, params.name)
+    observer = IntegralCatapult(params, params.name)
+
+    # Test moving the system.
+    initial_X = numpy.matrix([[0.0], [0.0]])
+    augmented_R = numpy.matrix(numpy.zeros((3, 1)))
+    augmented_R[0:2, :] = R
+    RunTest(plant,
+            end_goal=augmented_R,
+            controller=controller,
+            observer=observer,
+            duration=2.0,
+            use_profile=False,
+            kick_time=1.0,
+            kick_magnitude=2.0)
+
+
+def PlotMotion(params,
+               R,
+               max_velocity=10.0,
+               max_acceleration=70.0,
+               plant_params=None):
+    """Plots a trapezoidal motion.
+
+    Args:
+      params: CatapultParams for the controller and observer
+      plant_params: CatapultParams for the plant.  Defaults to params if
+        plant_params is None.
+      R: numpy.matrix(2, 1), the goal,
+      max_velocity: float, The max velocity of the profile.
+      max_acceleration: float, The max acceleration of the profile.
+    """
+    plant = Catapult(plant_params or params, params.name)
+    controller = IntegralCatapult(params, params.name)
+    observer = IntegralCatapult(params, params.name)
+
+    # Test moving the system.
+    initial_X = numpy.matrix([[0.0], [0.0]])
+    augmented_R = numpy.matrix(numpy.zeros((3, 1)))
+    augmented_R[0:2, :] = R
+    RunTest(plant,
+            end_goal=augmented_R,
+            controller=controller,
+            observer=observer,
+            duration=2.0,
+            use_profile=True,
+            max_velocity=max_velocity,
+            max_acceleration=max_acceleration)
+
+
 def WriteCatapult(params, plant_files, controller_files, year_namespaces):
     """Writes out the constants for a catapult to a file.
 
@@ -325,19 +267,18 @@
     if type(params) is list:
         name = params[0].name
         for index, param in enumerate(params):
-            catapults.append(
-                Catapult(param, param.name + str(index)))
+            catapults.append(Catapult(param, param.name + str(index)))
             integral_catapults.append(
-                IntegralCatapult(param,
-                                      'Integral' + param.name + str(index)))
+                IntegralCatapult(param, 'Integral' + param.name + str(index)))
     else:
         name = params.name
         catapults.append(Catapult(params, params.name))
         integral_catapults.append(
             IntegralCatapult(params, 'Integral' + params.name))
 
-    loop_writer = control_loop.ControlLoopWriter(
-        name, catapults, namespaces=year_namespaces)
+    loop_writer = control_loop.ControlLoopWriter(name,
+                                                 catapults,
+                                                 namespaces=year_namespaces)
     loop_writer.AddConstant(
         control_loop.Constant('kOutputRatio', '%f', catapults[0].G))
     loop_writer.AddConstant(