diff --git a/WORKSPACE b/WORKSPACE
index 2aeb3a3..855b31a 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -975,6 +975,22 @@
 )
 
 http_archive(
+    name = "osqp_amd64",
+    build_file = "@//debian:osqp_python.BUILD",
+    sha256 = "8003fc363f707daa46fef3af548e6a580372154d6cd49a7bf2f569ba5f807d15",
+    type = "zip",
+    url = "https://files.pythonhosted.org/packages/3f/e2/f1c40e890f00f8a566bc2481d0f215e52def3dfe8eea6b8ad4cc2d3cbca2/osqp-0.6.2.post5-cp39-cp39-manylinux_2_5_x86_64.manylinux1_x86_64.manylinux_2_17_x86_64.manylinux2014_x86_64.whl",
+)
+
+http_archive(
+    name = "qdldl_amd64",
+    build_file = "@//debian:qdldl_python.BUILD",
+    sha256 = "2c09f4b1a1c6f3a0579af004443417e084491e7c844ff9fb73170bb5d43f70b5",
+    type = "zip",
+    url = "https://files.pythonhosted.org/packages/9e/26/ccb4f065b40c1e9ff35ee66970d4fa97dd2fe221b846da2110eb8cd6c3f4/qdldl-0.1.5.post0-cp39-cp39-manylinux2014_x86_64.whl",
+)
+
+http_archive(
     name = "gstreamer_k8",
     build_file = "@//debian:gstreamer.BUILD",
     sha256 = "4d74d4a82f7a73dc9fe9463d5fae409b17845eef7cd64ef9c4c4553816c53589",
diff --git a/debian/osqp_python.BUILD b/debian/osqp_python.BUILD
new file mode 100644
index 0000000..9d3260e
--- /dev/null
+++ b/debian/osqp_python.BUILD
@@ -0,0 +1,15 @@
+py_library(
+    name = "python_osqp",
+    srcs = glob(["**/*.py"]),
+    data = glob(
+        include = ["**/*"],
+        exclude = ["**/*.py"],
+    ),
+    imports = ["."],
+    visibility = ["//visibility:public"],
+    deps = [
+        "@python_repo//:numpy",
+        "@python_repo//:scipy",
+        "@qdldl_amd64//:python_qdldl",
+    ],
+)
diff --git a/debian/qdldl_python.BUILD b/debian/qdldl_python.BUILD
new file mode 100644
index 0000000..76d647e
--- /dev/null
+++ b/debian/qdldl_python.BUILD
@@ -0,0 +1,14 @@
+py_library(
+    name = "python_qdldl",
+    srcs = glob(["**/*.py"]),
+    data = glob(
+        include = ["**/*"],
+        exclude = ["**/*.py"],
+    ),
+    imports = ["."],
+    visibility = ["//visibility:public"],
+    deps = [
+        "@python_repo//:numpy",
+        "@python_repo//:scipy",
+    ],
+)
diff --git a/tools/dependency_rewrite b/tools/dependency_rewrite
index f2172cc..d5190dd 100644
--- a/tools/dependency_rewrite
+++ b/tools/dependency_rewrite
@@ -7,6 +7,7 @@
 rewrite nodejs.org/(.*) software.frc971.org/Build-Dependencies/nodejs.org/$1
 rewrite static.rust-lang.org/(.*) software.frc971.org/Build-Dependencies/static.rust-lang.org/$1
 rewrite storage.googleapis.com/(.*) software.frc971.org/Build-Dependencies/storage.googleapis.com/$1
+rewrite files.pythonhosted.org/(.*) software.frc971.org/Build-Dependencies/files.pythonhosted.org/$1
 allow golang.org
 
 allow software.frc971.org
diff --git a/y2022/control_loops/python/BUILD b/y2022/control_loops/python/BUILD
index d3a5a62..286b874 100644
--- a/y2022/control_loops/python/BUILD
+++ b/y2022/control_loops/python/BUILD
@@ -72,6 +72,7 @@
         ":python_init",
         "//external:python-gflags",
         "//external:python-glog",
+        "@osqp_amd64//:python_osqp",
     ],
 )
 
diff --git a/y2022/control_loops/python/catapult.py b/y2022/control_loops/python/catapult.py
index ae6079a..491286e 100755
--- a/y2022/control_loops/python/catapult.py
+++ b/y2022/control_loops/python/catapult.py
@@ -3,6 +3,7 @@
 from frc971.control_loops.python import control_loop
 from frc971.control_loops.python import controls
 import numpy
+import osqp
 import math
 import scipy.optimize
 import sys
@@ -49,7 +50,7 @@
 print("J", J)
 
 kCatapult = catapult_lib.CatapultParams(
-    name='Finisher',
+    name='Catapult',
     motor=AddResistance(control_loop.NMotor(control_loop.Falcon(), 2), 0.03),
     G=G,
     J=J,
@@ -61,8 +62,6 @@
     controller_poles=[.93],
     dt=0.00505)
 
-catapult = catapult_lib.Catapult(kCatapult)
-
 # Ideas for adjusting the cost function:
 #
 # Penalize battery current?
@@ -76,8 +75,86 @@
 # We really want our cost function to be robust so that we can tolerate the
 # battery not delivering like we want at the end.
 
+def quadratic_cost(catapult, X_initial, X_final, horizon):
+    Q_final = numpy.matrix([[10000.0, 0.0], [0.0, 10000.0]])
 
-def mpc_cost(X_initial, X_final, u_matrix):
+    As = numpy.vstack([catapult.A**(n + 1) for n in range(0, horizon)])
+    Af = catapult.A**horizon
+
+    Bs = numpy.matrix(numpy.zeros((2 * horizon, horizon)))
+    for n in range(0, horizon):
+        for m in range(0, n + 1):
+            Bs[n * 2:(n * 2) + 2, m] = catapult.A**(n - m) * catapult.B
+
+    Bf = Bs[horizon * 2 - 2:, :]
+
+    P_final = 2.0 * Bf.transpose() * Q_final * Bf
+    q_final = (2.0 * (Af * X_initial - X_final).transpose() * Q_final *
+         Bf).transpose()
+
+    constant_final = (Af * X_initial - X_final).transpose() * Q_final * (
+        Af * X_initial - X_final)
+
+    m = numpy.matrix([[catapult.A[1, 1] ** (n + 1)] for n in range(horizon)])
+    M = Bs[1:horizon * 2:2, :]
+
+    W = numpy.matrix(numpy.identity(horizon) - numpy.eye(horizon, horizon, -1)) / catapult.dt
+    w = -numpy.matrix(numpy.eye(horizon, 1, 0)) / catapult.dt
+
+
+    Pi = numpy.diag([
+        (0.01 ** 2.0) + (0.02 * max(0.0, 20 - (horizon - n)) / 20.0) ** 2.0 for n in range(horizon)
+    ])
+
+    P_accel = 2.0 * M.transpose() * W.transpose() * Pi * W * M
+    q_accel = 2.0 * (((W * m + w) * X_initial[1, 0]).transpose() * Pi * W * M).transpose()
+    constant_accel = ((W * m + w) * X_initial[1, 0]).transpose() * Pi * (
+        (W * m + w) * X_initial[1, 0])
+
+    return ((P_accel + P_final), (q_accel + q_final), (constant_accel + constant_final))
+
+
+def new_cost(catapult, X_initial, X_final, u):
+    u_matrix = numpy.matrix(u).transpose()
+    Q_final = numpy.matrix([[10000.0, 0.0], [0.0, 10000.0]])
+
+    As = numpy.vstack([catapult.A**(n + 1) for n in range(0, len(u))])
+    Af = catapult.A**len(u)
+
+    Bs = numpy.matrix(numpy.zeros((2 * len(u), len(u))))
+    for n in range(0, len(u)):
+        for m in range(0, n + 1):
+            Bs[n * 2:(n * 2) + 2, m] = catapult.A**(n - m) * catapult.B
+
+    Bf = Bs[len(u) * 2 - 2:, :]
+
+    P_final = 2.0 * Bf.transpose() * Q_final * Bf
+    q_final = (2.0 * (Af * X_initial - X_final).transpose() * Q_final *
+         Bf).transpose()
+
+    constant_final = (Af * X_initial - X_final).transpose() * Q_final * (
+        Af * X_initial - X_final)
+
+    m = numpy.matrix([[catapult.A[1, 1] ** (n + 1)] for n in range(len(u))])
+    M = Bs[1:len(u) * 2:2, :]
+
+    W = numpy.matrix(numpy.identity(len(u)) - numpy.eye(len(u), len(u), -1)) / catapult.dt
+    w = -numpy.matrix(numpy.eye(len(u), 1, 0)) * X_initial[1, 0] / catapult.dt
+
+    accel = W * (M * u_matrix + m * X_initial[1, 0]) + w
+
+    Pi = numpy.diag([
+        (0.01 ** 2.0) + (0.02 * max(0.0, 20 - (len(u) - n)) / 20.0) ** 2.0 for n in range(len(u))
+    ])
+
+    P_accel = 2.0 * M.transpose() * W.transpose() * Pi * W * M
+    q_accel = 2.0 * ((W * m * X_initial[1, 0] + w).transpose() * Pi * W * M).transpose()
+    constant_accel = (W * m * X_initial[1, 0] +
+                      w).transpose() * Pi * (W * m * X_initial[1, 0] + w)
+
+
+def mpc_cost(catapult, X_initial, X_final, u_matrix):
+
     X = X_initial.copy()
     cost = 0.0
     last_u = u_matrix[0]
@@ -118,7 +195,7 @@
     return cost
 
 
-def SolveCatapult(X_initial, X_final, u):
+def SolveCatapult(catapult, X_initial, X_final, u):
     """ Solves for the optimal action given a seed, state, and target """
     def vbat_constraint(z, i):
         return 12.0 - z[i]
@@ -126,10 +203,31 @@
     def forward(z, i):
         return z[i]
 
-    def mpc_cost2(u_matrix):
-        return mpc_cost(X_initial, X_final, u_matrix)
+    P, q, c = quadratic_cost(catapult, X_initial, X_final, len(u))
 
-    constraints = [{
+
+    def mpc_cost2(u_solver):
+        u_matrix = numpy.matrix(u_solver).transpose()
+        cost = mpc_cost(catapult, X_initial, X_final, u_solver)
+        return cost
+
+
+    def mpc_cost3(u_solver):
+        u_matrix = numpy.matrix(u_solver).transpose()
+        return (0.5 * u_matrix.transpose() * P * u_matrix +
+                q.transpose() * u_matrix + c)[0, 0]
+
+    # If we provide scipy with the analytical jacobian and hessian, it solves
+    # more accurately and a *lot* faster.
+    def jacobian(u):
+        u_matrix = numpy.matrix(u).transpose()
+        return numpy.array(P * u_matrix + q)
+
+    def hessian(u):
+        return numpy.array(P)
+
+    constraints = []
+    constraints += [{
         'type': 'ineq',
         'fun': vbat_constraint,
         'args': (i, )
@@ -141,9 +239,12 @@
         'args': (i, )
     } for i in numpy.arange(len(u))]
 
-    result = scipy.optimize.minimize(mpc_cost2,
+    result = scipy.optimize.minimize(mpc_cost3,
                                      u,
+                                     jac=jacobian,
+                                     hess=hessian,
                                      method='SLSQP',
+                                     tol=1e-12,
                                      constraints=constraints)
     print(result)
 
@@ -171,7 +272,8 @@
     # Iteratively solve our MPC and simulate it.
     u_samples = []
     for i in range(kHorizon):
-        u_horizon = SolveCatapult(X, X_final, u)
+        u_horizon = SolveCatapult(c, X, X_final, u)
+
         u_samples.append(u_horizon[0])
         v_prior = X[1, 0]
         X = c.A * X + c.B * numpy.matrix([[u_horizon[0]]])
@@ -200,15 +302,43 @@
 def main(argv):
     # Do all our math with a lower voltage so we have headroom.
     U = numpy.matrix([[9.0]])
-    print(
-        "For G:", G, " max speed ",
-        catapult_lib.MaxSpeed(params=kCatapult,
-                              U=U,
-                              final_position=math.pi / 2.0))
 
-    CatapultProblem()
+    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(
+            "For G:", G, " max speed ",
+            catapult_lib.MaxSpeed(params=kCatapult,
+                                  U=U,
+                                  final_position=math.pi / 2.0))
+
+        CatapultProblem()
+        return 0
+
         catapult_lib.PlotShot(kCatapult, U, final_position=math.pi / 4.0)
 
         gs = []
@@ -222,7 +352,15 @@
                                       final_position=math.pi / 2.0))
         pylab.plot(gs, speed, label="max_speed")
         pylab.show()
-        return 0
+
+    if len(argv) != 5:
+        glog.fatal(
+            'Expected .h file name and .cc file name for the catapult and integral catapult.'
+        )
+    else:
+        namespaces = ['y2022', 'control_loops', 'superstructure', 'catapult']
+        catapult_lib.WriteCatapult(kCatapult, argv[1:3], argv[3:5], namespaces)
+    return 0
 
 
 if __name__ == '__main__':
diff --git a/y2022/control_loops/python/catapult_lib.py b/y2022/control_loops/python/catapult_lib.py
index 256793a..2606c44 100644
--- a/y2022/control_loops/python/catapult_lib.py
+++ b/y2022/control_loops/python/catapult_lib.py
@@ -306,3 +306,45 @@
     pylab.legend()
 
     pylab.show()
+
+def WriteCatapult(params, plant_files, controller_files, year_namespaces):
+    """Writes out the constants for a catapult to a file.
+
+    Args:
+      params: list of CatapultParams or CatapultParams, 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.
+      year_namespaces: list of strings, the namespace list to use.
+    """
+    # Write the generated constants out to a file.
+    catapults = []
+    integral_catapults = []
+
+    if type(params) is list:
+        name = params[0].name
+        for index, param in enumerate(params):
+            catapults.append(
+                Catapult(param, param.name + str(index)))
+            integral_catapults.append(
+                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.AddConstant(
+        control_loop.Constant('kOutputRatio', '%f', catapults[0].G))
+    loop_writer.AddConstant(
+        control_loop.Constant('kFreeSpeed', '%f',
+                              catapults[0].motor.free_speed))
+    loop_writer.Write(plant_files[0], plant_files[1])
+
+    integral_loop_writer = control_loop.ControlLoopWriter(
+        'Integral' + name, integral_catapults, namespaces=year_namespaces)
+    integral_loop_writer.Write(controller_files[0], controller_files[1])
diff --git a/y2022/control_loops/superstructure/catapult/mpc.tex b/y2022/control_loops/superstructure/catapult/mpc.tex
new file mode 100644
index 0000000..b7110fe
--- /dev/null
+++ b/y2022/control_loops/superstructure/catapult/mpc.tex
@@ -0,0 +1,202 @@
+\documentclass[a4paper,12pt]{article}
+\usepackage{amsmath}
+\usepackage{graphicx}
+\begin{document}
+
+TODO(austin): Now that the python matches the original problem and solves, confirm the paper matches what got implemented.
+
+osqp!
+
+\section{Catapult MPC}
+We want to phrase our problem as trying to solve for the set of control inputs
+which get us closest to the destination, but minimizes acceleration.
+Specifically, we want to minimize acceleration close to the end.
+We also have a voltage limit.
+
+Our model is
+
+\begin{equation}
+\label{cost}
+  \begin{bmatrix} x_1 \\ v_1 \end{bmatrix} =
+    \begin{bmatrix} a_{00} & a_{01} \\ 0 & a_{11} \end{bmatrix}
+      \begin{bmatrix} x_0 \\ v_0 \end{bmatrix} + 
+    \begin{bmatrix} b_{0} \\ b_{1} \end{bmatrix} \begin{bmatrix} u_0 \end{bmatrix}
+\end{equation}
+
+Our acceleration can be measured as:
+
+\begin{equation}
+\label{accel}
+  \frac{ \left( \boldsymbol{X_1(1)} - \boldsymbol{X_1(0)} \right)}{\Delta t}
+\end{equation}
+
+This simplifies to:
+
+\begin{equation}
+  \frac{a_{11} v_0 + b_1 u_0 - v_0}{\Delta t}
+\end{equation}
+
+and finally
+
+\begin{equation}
+  \frac{(a_{11} - 1) v_0 + b_1 u_0}{\Delta t}
+\end{equation}
+
+
+We can also compute our state matrix as a function of inital state and the control inputs.
+
+\begin{equation}
+\label{all_x}
+  \begin{bmatrix} X_1 \\ X_2 \\ X_3 \\ \vdots \end{bmatrix} = 
+  \begin{bmatrix} A  \\
+                  A^2  \\
+                  A^3  \\
+                  \vdots \end{bmatrix} 
+   X_0 + 
+  \begin{bmatrix} B & 0 & 0 & 0 \\
+                  A B & B & 0 & 0 \\
+                  A^2 B & A B & B & 0 \\
+                  \vdots  & \ddots & & \hdots \end{bmatrix} 
+  \begin{bmatrix} U_0 \\ U_1 \\ U_2 \\ \vdots \end{bmatrix}
+\end{equation}
+
+\section{MPC problem formulation}
+
+We want to penalize both final state and intermediate acceleration.  
+
+\begin{equation}
+C = \sum_{n=0}^{39} \frac{\left(v(n + 1) - v(n)\right)^2}{\Delta t} \pi_n + (X_{40} - X_{final})^T Q_{final} (X_{40} - X_{final})
+\end{equation}
+
+where $\pi_n$ is a constant only dependent on $n$, and designed such that it depends on the distance to the end of the sequence, not the distance from the start.
+
+In order to use OSQP, which has a code generator, we need to get this into the form of
+
+\begin{tabular}{ l l }
+minimize &      $ \frac{1}{2} X^T P X + q^T X $ \\
+subject to &    $ l <= A X <= u $ \\
+\end{tabular}
+
+This is the simplest form of a constrained quadratic program that we can solve efficiently.
+Luckily for us, the problem statement above fits that definition.
+
+\section{Manipulating the formulation}
+
+We need to separate constant factors from things dependent on U (or X in OSQP parlance) so we can create these matrices easier.
+
+\subsection{Terminal cost}
+
+Next step is to compute $X_{40}$ using equation \ref{all_x}.
+We can do this by only computing the final row of the matrix.
+
+\begin{equation}
+\label{x40}
+  X_{40} = \begin{bmatrix} A^{39} B & A^{38} B & \hdots & B \end{bmatrix}
+           \begin{bmatrix} U_0 \\
+                           \vdots \\
+                           U_{39}
+           \end{bmatrix} + A^{40} X_0 = B_f \boldsymbol{U} + A^{40} X_0
+\end{equation}
+
+We can substitute equation \ref{x40} into equation \ref{cost}.
+
+\begin{equation}
+\label{final_cost}
+\begin{aligned}[t]
+  C_f = & \boldsymbol{U}^T B_f^T Q_{final} B_f \boldsymbol{U} \\
+        & + 2 (A^{40} X_0 - X_{final})^T Q_{final} B_f \boldsymbol{U} \\
+        & + (A^{40} X_0 - X_{final})^T Q_{final} (A^{40} X_0 - X_{final})
+\end{aligned}
+\end{equation}
+
+\subsection{Acceleration costs}
+
+We can compute a velocity matrix for all the times by stripping out the positions from equation \ref{all_x} by using every other row.
+We can use this to then compute the accelerations for each time slice and then penalize them.
+
+\begin{equation}
+  \begin{bmatrix} v_1 \\ v_2 \\ \vdots \\ v_{40} \end{bmatrix} =
+     M \boldsymbol{U} + \begin{bmatrix} a_{11} \\ a^2_{11} \\ \vdots \\ a^{40}_{11} \end{bmatrix} v_0 =
+     M \boldsymbol{U} + m v_0
+\end{equation}
+
+We can then use equation \ref{accel} in matrix form to convert a velocity matrix to an acceleration matrix.
+
+\begin{equation}
+  \begin{bmatrix} \alpha_1 \\ \alpha_2 \\ \alpha_3 \\ \vdots \\ \alpha_{40} \end{bmatrix} =
+    \frac{1}{\Delta t} \left(
+    \begin{bmatrix} 1 & 0 & 0 & \hdots & 0 \\
+                    -1 & 1 & 0 & \hdots & 0 \\
+                    0 & -1 & 1 & \hdots & 0 \\
+                    \vdots & & & \ddots  & \vdots \\
+                    0 & 0 & \hdots & -1 & 1 \\
+  \end{bmatrix}
+  \begin{bmatrix} v_1 \\ v_2 \\ \vdots \\ v_{40} \end{bmatrix} - \begin{bmatrix} v_0 \\ 0 \\ \vdots \\ 0 \end{bmatrix}
+\right)
+\end{equation}
+
+We can pull some of these terms out to make it easier to work with.
+
+\begin{equation}
+\boldsymbol{\alpha} = W V + w v_0
+\end{equation}
+
+Our acceleration cost function is then:
+
+\begin{equation}
+C_a = \boldsymbol{\alpha}^T
+        \begin{bmatrix} \pi_1 &  & 0 \\
+                        & \pi_2 &   \\
+                        0 & & \ddots \end{bmatrix} \boldsymbol{\alpha} = 
+ \boldsymbol{\alpha}^T \Pi \boldsymbol{\alpha}
+\end{equation}
+
+We can substitute everything in to get something as a function of $U$.
+
+\begin{equation}
+C_a = \left(W \left(M \boldsymbol{U} + m v_0 \right) + w v_0 \right)^T \Pi \left(W \left(M \boldsymbol{U} + m v_0 \right) + w v_0 \right)
+\end{equation}
+
+And then simplify this down into the expected form.
+
+\begin{equation}
+  C_a = \left(W M \boldsymbol{U} + (W m + w) v_0 \right)^T \Pi \left(W M \boldsymbol{U} + (W m + w) v_0  \right)
+\end{equation}
+
+\begin{equation}
+\label{accel_cost}
+\begin{aligned}[t]
+C_a = & \boldsymbol{U}^T M^T W^T \Pi W M \boldsymbol{U} \\
+  & + 2 v_0 (W m + w)^T \Pi W M \boldsymbol{U} \\
+& +  v_0 (W m + w)^T  \Pi \left(W m + w \right) v_0
+\end{aligned}
+\end{equation}
+
+\subsection{Overall cost}
+
+We can combine equations \ref{final_cost} and \ref{accel_cost} to get our overall cost in the correct form.
+
+\begin{equation}
+\begin{aligned}[t]
+C &= \boldsymbol{U}^T \left( M^T W^T \Pi W M + B_f^T Q_{final} B_f \right) \boldsymbol{U}  \\
+ &+ \left(2 v_0 (W m + w)^T \Pi W M
+   - 2 X_{final}^T Q_{final} B_f
+\right) \boldsymbol{U} \\
+& + X_{final}^T Q_{final} X_{final}
+  +  v_0 (W m + w)^T  \Pi \left(W m + w \right) v_0
+\end{aligned}
+\end{equation}
+
+
+\section{Response}
+
+For a reasonable request (11 m/s after 90 degrees), we get the following response
+
+\begin{figure}
+    \centering
+    \includegraphics[width=\linewidth]{response.png}
+\end{figure}
+
+This is well within 1\% error, and avoid saturation and keeps the acceleration down at the end.
+
+\end{document}