diff --git a/frc971/control_loops/swerve/BUILD b/frc971/control_loops/swerve/BUILD
index 09b363a..ed2c604 100644
--- a/frc971/control_loops/swerve/BUILD
+++ b/frc971/control_loops/swerve/BUILD
@@ -165,3 +165,16 @@
         "@pip//scipy",
     ],
 )
+
+py_test(
+    name = "physics_test",
+    srcs = [
+        "dynamics.py",
+        "physics_test.py",
+    ],
+    target_compatible_with = ["@platforms//cpu:x86_64"],
+    deps = [
+        "@pip//casadi",
+        "@pip//numpy",
+    ],
+)
diff --git a/frc971/control_loops/swerve/generate_physics.cc b/frc971/control_loops/swerve/generate_physics.cc
index 1761308..9f0f5b7 100644
--- a/frc971/control_loops/swerve/generate_physics.cc
+++ b/frc971/control_loops/swerve/generate_physics.cc
@@ -25,8 +25,8 @@
 
 DEFINE_string(output_base, "",
               "Path to strip off the front of the output paths.");
-DEFINE_string(cc_output_path, "", "Path to write generated header code to");
-DEFINE_string(h_output_path, "", "Path to write generated cc code to");
+DEFINE_string(cc_output_path, "", "Path to write generated cc code to");
+DEFINE_string(h_output_path, "", "Path to write generated header code to");
 DEFINE_string(py_output_path, "", "Path to write generated py code to");
 DEFINE_string(casadi_py_output_path, "",
               "Path to write casadi generated py code to");
@@ -76,6 +76,7 @@
   RCP<const Symbol> alphad;
   RCP<const Basic> alphad_eqn;
 
+  DenseMatrix contact_patch_velocity;
   DenseMatrix wheel_ground_velocity;
   RCP<const Basic> slip_angle;
   RCP<const Basic> slip_ratio;
@@ -99,7 +100,7 @@
       Cx_ = symbol("Cx");
       Cy_ = symbol("Cy");
 
-      r_w_ = symbol("r_w_");
+      rw_ = symbol("rw");
 
       m_ = symbol("m");
       J_ = symbol("J");
@@ -133,7 +134,7 @@
       Cx_ = real_double(5 * 9.8 / 0.05 / 4.0);
       Cy_ = real_double(5 * 9.8 / 0.05 / 4.0);
 
-      r_w_ = real_double(2 * 0.0254);
+      rw_ = real_double(2 * 0.0254);
 
       m_ = real_double(25.0);  // base is 20 kg without battery
       J_ = real_double(6.0);
@@ -462,7 +463,8 @@
   void WriteCasadiVariables(std::vector<std::string> *result_py) {
     result_py->emplace_back("    sin = casadi.sin");
     result_py->emplace_back("    cos = casadi.cos");
-    result_py->emplace_back("    atan2 = casadi.atan2");
+    result_py->emplace_back("    atan2 = half_atan2");
+    result_py->emplace_back("    fmax = casadi.fmax");
     result_py->emplace_back("    fabs = casadi.fabs");
 
     // Start by writing out variables matching each of the symbol names we use
@@ -510,7 +512,17 @@
     result_py.emplace_back("");
     result_py.emplace_back("import casadi");
     result_py.emplace_back("");
-    result_py.emplace_back("# Returns the derivative of our state vector");
+    result_py.emplace_back(absl::Substitute("WHEEL_RADIUS = $0", ccode(*rw_)));
+    result_py.emplace_back(
+        absl::Substitute("ROBOT_WIDTH = $0", ccode(*robot_width_)));
+    result_py.emplace_back(absl::Substitute("CASTER = $0", ccode(*caster_)));
+    result_py.emplace_back("");
+    result_py.emplace_back("def half_atan2(y, x):");
+    result_py.emplace_back(
+        "    return casadi.fmod(casadi.atan2(y, x) + casadi.pi * 3.0 / 2.0, "
+        "casadi.pi) - casadi.pi / 2.0");
+    result_py.emplace_back("");
+
     result_py.emplace_back("# Returns the derivative of our state vector");
     result_py.emplace_back("# [thetas0, thetad0, omegas0, omegad0,");
     result_py.emplace_back("#  thetas1, thetad1, omegas1, omegad1,");
@@ -567,6 +579,32 @@
     result_py.emplace_back("");
     result_py.emplace_back(
         "# Returns the velocity of the wheel in steer module coordinates.");
+    result_py.emplace_back("def contact_patch_velocity(i, X, U):");
+    WriteCasadiVariables(&result_py);
+    result_py.emplace_back(
+        "    result = casadi.SX.sym('contact_patch_velocity', 2, 1)");
+
+    for (size_t m = 0; m < kNumModules; ++m) {
+      if (m == 0) {
+        result_py.emplace_back("    if i == 0:");
+      } else {
+        result_py.emplace_back(absl::Substitute("    elif i == $0:", m));
+      }
+      for (int j = 0; j < 2; ++j) {
+        result_py.emplace_back(absl::Substitute(
+            "        result[$0, 0] = $1", j,
+            ccode(*modules_[m].contact_patch_velocity.get(j, 0))));
+      }
+    }
+    result_py.emplace_back("    else:");
+    result_py.emplace_back(
+        "        raise ValueError(\"Invalid module number\")");
+    result_py.emplace_back(
+        "    return casadi.Function('contact_patch_velocity', [X, U], "
+        "[result])");
+    result_py.emplace_back("");
+    result_py.emplace_back(
+        "# Returns the velocity of the wheel in steer module coordinates.");
     result_py.emplace_back("def wheel_ground_velocity(i, X, U):");
     WriteCasadiVariables(&result_py);
     result_py.emplace_back(
@@ -581,13 +619,15 @@
       for (int j = 0; j < 2; ++j) {
         result_py.emplace_back(absl::Substitute(
             "        result[$0, 0] = $1", j,
-            ccode(*modules_[m].wheel_ground_velocity.get(0, 0))));
+            ccode(*modules_[m].wheel_ground_velocity.get(j, 0))));
       }
     }
     result_py.emplace_back("    else:");
     result_py.emplace_back(
         "        raise ValueError(\"Invalid module number\")");
-    result_py.emplace_back("    return result");
+    result_py.emplace_back(
+        "    return casadi.Function('wheel_ground_velocity', [X, U], "
+        "[result])");
 
     result_py.emplace_back("");
     result_py.emplace_back("# Returns the slip angle of the ith wheel.");
@@ -599,8 +639,9 @@
       } else {
         result_py.emplace_back(absl::Substitute("    elif i == $0:", m));
       }
-      result_py.emplace_back(absl::Substitute("        return $0",
-                                              ccode(*modules_[m].slip_angle)));
+      result_py.emplace_back(absl::Substitute(
+          "        return casadi.Function('slip_angle', [X, U], [$0])",
+          ccode(*modules_[m].slip_angle)));
     }
     result_py.emplace_back("    raise ValueError(\"Invalid module number\")");
     result_py.emplace_back("");
@@ -613,8 +654,9 @@
       } else {
         result_py.emplace_back(absl::Substitute("    elif i == $0:", m));
       }
-      result_py.emplace_back(absl::Substitute("        return $0",
-                                              ccode(*modules_[m].slip_ratio)));
+      result_py.emplace_back(absl::Substitute(
+          "        return casadi.Function('slip_ratio', [X, U], [$0])",
+          ccode(*modules_[m].slip_ratio)));
     }
     result_py.emplace_back("    raise ValueError(\"Invalid module number\")");
 
@@ -639,7 +681,8 @@
     result_py.emplace_back("    else:");
     result_py.emplace_back(
         "        raise ValueError(\"Invalid module number\")");
-    result_py.emplace_back("    return result");
+    result_py.emplace_back(
+        "    return casadi.Function('wheel_force', [X, U], [result])");
 
     aos::util::WriteStringToFileOrDie(py_path, absl::StrJoin(result_py, "\n"));
   }
@@ -668,14 +711,13 @@
     result.alphad = symbol(absl::StrFormat("alphad%u", m));
 
     // Velocity of the module in field coordinates
-    DenseMatrix robot_velocity = DenseMatrix(2, 1);
-    mul_dense_dense(R(theta_), DenseMatrix(2, 1, {vx_, vy_}), robot_velocity);
+    DenseMatrix robot_velocity = DenseMatrix(2, 1, {vx_, vy_});
     VLOG(1) << "robot velocity: " << robot_velocity.__str__();
 
     // Velocity of the contact patch in field coordinates
     DenseMatrix temp_matrix = DenseMatrix(2, 1);
     DenseMatrix temp_matrix2 = DenseMatrix(2, 1);
-    DenseMatrix contact_patch_velocity = DenseMatrix(2, 1);
+    result.contact_patch_velocity = DenseMatrix(2, 1);
 
     mul_dense_dense(R(theta_), mounting_location, temp_matrix);
     add_dense_dense(angle_cross(temp_matrix, omega_), robot_velocity,
@@ -684,14 +726,16 @@
                     DenseMatrix(2, 1, {caster_, integer(0)}), temp_matrix);
     add_dense_dense(temp_matrix2,
                     angle_cross(temp_matrix, add(omega_, result.omegas)),
-                    contact_patch_velocity);
+                    result.contact_patch_velocity);
 
     VLOG(1);
-    VLOG(1) << "contact patch velocity: " << contact_patch_velocity.__str__();
+    VLOG(1) << "contact patch velocity: "
+            << result.contact_patch_velocity.__str__();
 
     // Relative velocity of the surface of the wheel to the ground.
     result.wheel_ground_velocity = DenseMatrix(2, 1);
-    mul_dense_dense(R(neg(add(result.thetas, theta_))), contact_patch_velocity,
+    mul_dense_dense(R(neg(add(result.thetas, theta_))),
+                    result.contact_patch_velocity,
                     result.wheel_ground_velocity);
 
     VLOG(1);
@@ -704,9 +748,11 @@
     VLOG(1);
     VLOG(1) << "slip angle: " << result.slip_angle->__str__();
 
+    // TODO(austin): Does this handle decel properly?
     result.slip_ratio = div(
-        sub(mul(r_w_, result.omegad), result.wheel_ground_velocity.get(0, 0)),
-        abs(result.wheel_ground_velocity.get(0, 0)));
+        sub(mul(rw_, result.omegad), result.wheel_ground_velocity.get(0, 0)),
+        SymEngine::max(
+            {real_double(0.02), abs(result.wheel_ground_velocity.get(0, 0))}));
     VLOG(1);
     VLOG(1) << "Slip ratio " << result.slip_ratio->__str__();
 
@@ -725,7 +771,7 @@
     // alphas = ...
     RCP<const Basic> lhms =
         mul(add(neg(wb_), mul(add(rs_, rp_), sub(integer(1), div(rb1_, rp_)))),
-            mul(div(r_w_, rb2_), neg(result.Fwx)));
+            mul(div(rw_, rb2_), neg(result.Fwx)));
     RCP<const Basic> lhs = add(add(Ms, div(mul(Jsm_, result.Is), Gs_)), lhms);
     RCP<const Basic> rhs = add(Jsm_, div(div(Js_, Gs_), Gs_));
     RCP<const Basic> accel_steer_eqn = simplify(div(lhs, rhs));
@@ -743,7 +789,7 @@
 
     RCP<const Basic> drive_eqn = sub(
         add(mul(neg(Jdm_), div(alphamd, Gd_)), mul(Ktd_, div(result.Id, Gd_))),
-        mul(neg(result.Fwx), r_w_));
+        mul(neg(result.Fwx), rw_));
 
     VLOG(1) << "drive_eqn: " << drive_eqn->__str__();
 
@@ -771,7 +817,7 @@
     mul_dense_dense(R(add(theta_, result.thetas)),
                     DenseMatrix(2, 1, {result.Fwx, result.Fwy}), mat_output);
 
-    // Comput the resulting force from the module.
+    // Compute the resulting force from the module.
     DenseMatrix F = mat_output;
 
     RCP<const Basic> torque = simplify(force_cross(mounting_location, F));
@@ -797,7 +843,7 @@
   }
 
   DenseMatrix angle_cross(DenseMatrix a, RCP<const Basic> b) {
-    return DenseMatrix(2, 1, {mul(a.get(1, 0), b), mul(neg(a.get(0, 0)), b)});
+    return DenseMatrix(2, 1, {mul(neg(a.get(1, 0)), b), mul(a.get(0, 0), b)});
   }
 
   RCP<const Basic> force_cross(DenseMatrix r, DenseMatrix f) {
@@ -822,7 +868,7 @@
 
   RCP<const Basic> Cx_;
   RCP<const Basic> Cy_;
-  RCP<const Basic> r_w_;
+  RCP<const Basic> rw_;
   RCP<const Basic> m_;
   RCP<const Basic> J_;
   RCP<const Basic> Gd1_;
diff --git a/frc971/control_loops/swerve/physics_test.py b/frc971/control_loops/swerve/physics_test.py
new file mode 100644
index 0000000..7ea7123
--- /dev/null
+++ b/frc971/control_loops/swerve/physics_test.py
@@ -0,0 +1,194 @@
+#!/usr/bin/python3
+
+import numpy
+import sys, os
+import casadi
+from numpy.testing import assert_array_equal, assert_array_almost_equal
+import unittest
+
+from frc971.control_loops.swerve import dynamics
+
+
+def state_vector(velocity=numpy.array([[1.0], [0.0]]),
+                 dx=0.0,
+                 dy=0.0,
+                 theta=0.0,
+                 omega=0.0,
+                 module_omega=0.0,
+                 module_angle=0.0):
+    """Returns the state vector with the requested state."""
+    X_initial = numpy.zeros((25, 1))
+    # All the wheels are spinning at the speed needed to hit the velocity in m/s
+    X_initial[2, 0] = module_omega
+    X_initial[3, 0] = numpy.linalg.norm(velocity) / (dynamics.WHEEL_RADIUS)
+
+    X_initial[6, 0] = module_omega
+    X_initial[7, 0] = numpy.linalg.norm(velocity) / (dynamics.WHEEL_RADIUS)
+
+    X_initial[10, 0] = module_omega
+    X_initial[11, 0] = numpy.linalg.norm(velocity) / (dynamics.WHEEL_RADIUS)
+
+    X_initial[14, 0] = module_omega
+    X_initial[15, 0] = numpy.linalg.norm(velocity) / (dynamics.WHEEL_RADIUS)
+
+    X_initial[0, 0] = module_angle
+    X_initial[4, 0] = module_angle
+    X_initial[8, 0] = module_angle
+    X_initial[12, 0] = module_angle
+
+    X_initial[18, 0] = theta
+
+    X_initial[19, 0] = velocity[0, 0] + dx
+    X_initial[20, 0] = velocity[1, 0] + dy
+    X_initial[21, 0] = omega
+
+    return X_initial
+
+
+class TestHPolytope(unittest.TestCase):
+    I = numpy.zeros((8, 1))
+
+    def setUp(self):
+        pass
+
+    def test_contact_patch_velocity(self):
+        """Tests that the contact patch velocity makes sense."""
+        for i in range(4):
+            contact_patch_velocity = dynamics.contact_patch_velocity(
+                i, casadi.SX.sym("X", 25, 1), casadi.SX.sym("U", 8, 1))
+            wheel_ground_velocity = dynamics.wheel_ground_velocity(
+                i, casadi.SX.sym("X", 25, 1), casadi.SX.sym("U", 8, 1))
+
+            # No angular velocity should result in just linear motion.
+            for velocity in [
+                    numpy.array([[1.5], [0.0]]),
+                    numpy.array([[0.0], [1.0]]),
+                    numpy.array([[-1.5], [0.0]]),
+                    numpy.array([[0.0], [-1.0]]),
+                    numpy.array([[2.0], [-1.7]]),
+            ]:
+                for theta in [0.0, 1.0, -1.0, 100.0]:
+                    patch_velocity = numpy.array(
+                        contact_patch_velocity(
+                            state_vector(velocity=velocity, theta=theta),
+                            self.I))
+
+                    assert_array_equal(patch_velocity, velocity)
+
+            # Now, test that spinning the robot results in module velocities.
+            # We are assuming that each module is on a square robot.
+            module_center_of_mass_angle = i * numpy.pi / 2.0 + numpy.pi / 4.0
+            for theta in [-module_center_of_mass_angle, 0.0, 1.0, -1.0, 100.0]:
+                for omega in [0.65, -0.1]:
+                    # Point each module to the center to make the math easier.
+                    patch_velocity = numpy.array(
+                        contact_patch_velocity(
+                            state_vector(
+                                velocity=numpy.array([[0.0], [0.0]]),
+                                theta=theta,
+                                omega=omega,
+                                module_angle=module_center_of_mass_angle),
+                            self.I))
+
+                    assert_array_almost_equal(
+                        patch_velocity,
+                        (dynamics.ROBOT_WIDTH / numpy.sqrt(2.0) +
+                         dynamics.CASTER) * omega * numpy.array([[
+                             -numpy.sin(theta + module_center_of_mass_angle)
+                         ], [numpy.cos(theta + module_center_of_mass_angle)]]))
+
+                    # Point the wheel along +x, rotate it by theta, then spin it.
+                    # Confirm the velocities come out right.
+                    patch_velocity = numpy.array(
+                        contact_patch_velocity(
+                            state_vector(
+                                velocity=numpy.array([[0.0], [0.0]]),
+                                theta=-module_center_of_mass_angle,
+                                module_omega=omega,
+                                module_angle=(theta +
+                                              module_center_of_mass_angle)),
+                            self.I))
+
+                    assert_array_almost_equal(
+                        patch_velocity,
+                        dynamics.CASTER * omega *
+                        numpy.array([[-numpy.sin(theta)], [numpy.cos(theta)]]))
+
+            # Now, test that the rotation back to wheel coordinates works.
+            # The easiest way to do this is to point the wheel in a direction,
+            # move in that direction, and confirm that there is no lateral velocity.
+            for robot_angle in [0.0, 1.0, -5.0]:
+                for module_angle in [0.0, 1.0, -5.0]:
+                    wheel_patch_velocity = numpy.array(
+                        wheel_ground_velocity(
+                            state_vector(velocity=numpy.array(
+                                [[numpy.cos(robot_angle + module_angle)],
+                                 [numpy.sin(robot_angle + module_angle)]]),
+                                         theta=robot_angle,
+                                         module_angle=module_angle), self.I))
+
+                    assert_array_almost_equal(wheel_patch_velocity,
+                                              numpy.array([[1], [0]]))
+
+    def test_slip_angle(self):
+        """Tests that the slip_angle calculation works."""
+        velocity = numpy.array([[1.5], [0.0]])
+
+        for i in range(4):
+            x = casadi.SX.sym("x")
+            y = casadi.SX.sym("y")
+            half_atan2 = casadi.Function('half_atan2', [y, x],
+                                         [dynamics.half_atan2(y, x)])
+
+            slip_angle = dynamics.slip_angle(i, casadi.SX.sym("X", 25, 1),
+                                             casadi.SX.sym("U", 8, 1))
+
+            for wrap in range(-3, 3):
+                for theta in [0.0, 0.6, -0.4]:
+                    module_angle = numpy.pi * wrap + theta
+
+                    self.assertAlmostEqual(
+                        theta,
+                        half_atan2(numpy.sin(module_angle),
+                                   numpy.cos(module_angle)))
+
+                    computed_angle = slip_angle(
+                        state_vector(velocity=velocity,
+                                     module_angle=numpy.pi * wrap + theta),
+                        self.I)
+
+                    self.assertAlmostEqual(theta, computed_angle)
+
+    def test_wheel_forces(self):
+        """Tests that the per module forces have the right signs."""
+        for i in range(4):
+            wheel_force = dynamics.wheel_force(i, casadi.SX.sym("X", 25, 1),
+                                               casadi.SX.sym("U", 8, 1))
+
+            # Robot is moving faster than the wheels, it should decelerate.
+            robot_faster = numpy.array(
+                wheel_force(state_vector(dx=0.01), self.I))
+            self.assertLess(robot_faster[0, 0], -0.1)
+            self.assertEqual(robot_faster[1, 0], 0.0)
+
+            # Robot is now going slower than the wheels.  It should accelerate.
+            robot_slower = numpy.array(
+                wheel_force(state_vector(dx=-0.01), self.I))
+            self.assertGreater(robot_slower[0, 0], 0.1)
+            self.assertEqual(robot_slower[1, 0], 0.0)
+
+            # Positive lateral velocity -> negative force.
+            robot_left = numpy.array(wheel_force(state_vector(dy=0.01),
+                                                 self.I))
+            self.assertEqual(robot_left[0, 0], 0.0)
+            self.assertLess(robot_left[1, 0], -0.1)
+
+            # Negative lateral velocity -> positive force.
+            robot_right = numpy.array(
+                wheel_force(state_vector(dy=-0.01), self.I))
+            self.assertEqual(robot_right[0, 0], 0.0)
+            self.assertGreater(robot_right[1, 0], 0.1)
+
+
+if __name__ == '__main__':
+    unittest.main()