diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 1480e4e..a1b96be 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -106,6 +106,7 @@
         ":spline",
         ":trajectory",
         "//aos:init",
+        "//aos/util:math",
     ],
 )
 
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 73ebe10..09ecb4f 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -130,6 +130,8 @@
 
     // Which spline to follow.
     int32_t spline_handle;
+    // Whether to follow the spline driving forwards or backwards.
+    bool drive_spline_backwards;
   };
 
   message Position {
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 05889e4..9d031b7 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -118,8 +118,8 @@
     double expected_x = my_drivetrain_queue_.status->trajectory_logging.x;
     double expected_y = my_drivetrain_queue_.status->trajectory_logging.y;
     auto actual = drivetrain_motor_plant_.GetPosition();
-    EXPECT_NEAR(actual(0), expected_x, 1e-2);
-    EXPECT_NEAR(actual(1), expected_y, 1e-2);
+    EXPECT_NEAR(actual(0), expected_x, 2e-2);
+    EXPECT_NEAR(actual(1), expected_y, 2e-2);
   }
 
   void WaitForTrajectoryPlan() {
@@ -435,6 +435,30 @@
   VerifyNearSplineGoal();
 }
 
+// Tests that we can drive a spline backwards.
+TEST_F(DrivetrainTest, SplineSimpleBackwards) {
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  goal->drive_spline_backwards = true;
+  goal->controller_type = 2;
+  goal->spline.spline_idx = 1;
+  goal->spline.spline_count = 1;
+  goal->spline.spline_x = {{0.0, -0.25, -0.5, -0.5, -0.75, -1.0}};
+  goal->spline.spline_y = {{0.0, 0.0, -0.25 ,-0.75, -1.0, -1.0}};
+  goal.Send();
+  RunIteration();
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+      start_goal = my_drivetrain_queue_.goal.MakeMessage();
+  start_goal->controller_type = 2;
+  start_goal->spline_handle = 1;
+  start_goal.Send();
+  WaitForTrajectoryPlan();
+
+  RunForTime(chrono::milliseconds(2000));
+  VerifyNearSplineGoal();
+}
+
 // Tests that simple spline with a single goal message.
 TEST_F(DrivetrainTest, SplineSingleGoal) {
   ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
@@ -528,8 +552,8 @@
   goal->controller_type = 2;
   goal->spline.spline_idx = 1;
   goal->spline.spline_count = 1;
-  goal->spline.spline_x = {{0.5, 0.25, 0.5, 0.5, 0.75, 1.0}};
-  goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+  goal->spline.spline_x = {{0.2, 0.25, 0.5, 0.5, 0.75, 1.0}};
+  goal->spline.spline_y = {{0.2, 0.0, 0.25 ,0.75, 1.0, 1.0}};
   goal.Send();
   RunIteration();
 
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index a37ca0e..18c8381 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -3,6 +3,7 @@
 #include "Eigen/Dense"
 
 #include "aos/init.h"
+#include "aos/util/math.h"
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
 
@@ -99,6 +100,7 @@
 
   ::aos::Mutex::State mutex_state = mutex_.TryLock();
   if (mutex_state == ::aos::Mutex::State::kLocked) {
+    drive_spline_backwards_ = goal_.drive_spline_backwards;
     if (goal.spline.spline_idx && future_spline_idx_ != goal.spline.spline_idx) {
       goal_ = goal;
       new_goal_.Broadcast();
@@ -143,7 +145,17 @@
     ::Eigen::Matrix<double, 2, 5> K =
         current_trajectory_->KForState(state, dt_config_.dt, Q, R);
     ::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
+    if (drive_spline_backwards_) {
+      ::Eigen::Matrix<double, 2, 1> swapU(U_ff(1, 0), U_ff(0, 0));
+      U_ff = -swapU;
+      goal_state(2, 0) += M_PI;
+      double left_goal = goal_state(3, 0);
+      double right_goal = goal_state(4, 0);
+      goal_state(3, 0) = -right_goal;
+      goal_state(4, 0) = -left_goal;
+    }
     ::Eigen::Matrix<double, 5, 1> state_error = goal_state - state;
+    state_error(2, 0) = ::aos::math::NormalizeAngle(state_error(2, 0));
     ::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
 
     ::Eigen::Matrix<double, 2, 1> xv_state = current_xva_.block<2,1>(0,0);
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index d70cb6d..59175b6 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -71,6 +71,7 @@
   int32_t current_spline_handle_ = 0;  // Current spline told to excecute.
   int32_t current_spline_idx_ = 0;     // Current executing spline.
   bool has_started_execution_ = false;
+  bool drive_spline_backwards_ = false;
 
   ::std::unique_ptr<DistanceSpline> current_distance_spline_;
   ::std::unique_ptr<Trajectory> current_trajectory_;
@@ -105,8 +106,8 @@
   // TODO(alex): pull this out of dt_config.
   const ::Eigen::DiagonalMatrix<double, 5> Q =
       (::Eigen::DiagonalMatrix<double, 5>().diagonal()
-           << 1.0 / ::std::pow(0.07, 2),
-       1.0 / ::std::pow(0.07, 2), 1.0 / ::std::pow(0.2, 2),
+           << 1.0 / ::std::pow(0.12, 2),
+       1.0 / ::std::pow(0.12, 2), 1.0 / ::std::pow(0.1, 2),
        1.0 / ::std::pow(1.5, 2), 1.0 / ::std::pow(1.5, 2))
           .finished()
           .asDiagonal();