diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index b08e96f..4e6f373 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -643,6 +643,7 @@
         ":drivetrain_config",
         ":spline_goal_fbs",
         ":trajectory_fbs",
+        "//aos/util:math",
         "//frc971/control_loops:c2d",
         "//frc971/control_loops:dlqr",
         "//frc971/control_loops:hybrid_state_feedback_loop",
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 7dd5a52..d57e81e 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -811,9 +811,25 @@
       CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->has_y());
 }
 
+class DrivetrainBackwardsParamTest
+    : public DrivetrainTest,
+      public ::testing::WithParamInterface<bool> {};
+
 // Tests that simple spline converges when it doesn't start where it thinks.
-TEST_F(DrivetrainTest, SplineOffset) {
+TEST_P(DrivetrainBackwardsParamTest, SplineOffset) {
   SetEnabled(true);
+  if (GetParam()) {
+    // Turn the robot around if we are backwards.
+    (*drivetrain_plant_.mutable_state())(2) += M_PI;
+
+    auto builder = localizer_control_sender_.MakeBuilder();
+    LocalizerControl::Builder localizer_control_builder =
+        builder.MakeBuilder<LocalizerControl>();
+    localizer_control_builder.add_x(drivetrain_plant_.state()(0));
+    localizer_control_builder.add_y(drivetrain_plant_.state()(1));
+    localizer_control_builder.add_theta(drivetrain_plant_.state()(2));
+    ASSERT_TRUE(builder.Send(localizer_control_builder.Finish()));
+  }
   {
     auto builder = trajectory_goal_sender_.MakeBuilder();
 
@@ -834,7 +850,7 @@
 
     SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
     spline_goal_builder.add_spline_idx(1);
-    spline_goal_builder.add_drive_spline_backwards(false);
+    spline_goal_builder.add_drive_spline_backwards(GetParam());
     spline_goal_builder.add_spline(multispline_offset);
     ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
   }
@@ -852,6 +868,10 @@
   VerifyNearSplineGoal();
 }
 
+INSTANTIATE_TEST_CASE_P(DriveSplinesForwardsAndBackwards,
+                        DrivetrainBackwardsParamTest,
+                        ::testing::Values(false, true));
+
 // Tests that simple spline converges when it starts to the side of where it
 // thinks.
 TEST_F(DrivetrainTest, SplineSideOffset) {
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index cdedce0..fd0ad6e 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -117,7 +117,8 @@
     ::Eigen::Matrix<double, 2, 5> K =
         current_trajectory().GainForDistance(current_distance);
     ::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
-    if (current_trajectory().drive_spline_backwards()) {
+    const bool backwards = current_trajectory().drive_spline_backwards();
+    if (backwards) {
       ::Eigen::Matrix<double, 2, 1> swapU(U_ff(1, 0), U_ff(0, 0));
       U_ff = -swapU;
       goal_state(2, 0) += M_PI;
@@ -128,13 +129,19 @@
     }
     const Eigen::Matrix<double, 5, 1> relative_goal =
         current_trajectory().StateToPathRelativeState(current_distance,
-                                                      goal_state);
+                                                      goal_state, backwards);
     const Eigen::Matrix<double, 5, 1> relative_state =
-        current_trajectory().StateToPathRelativeState(current_distance, state);
+        current_trajectory().StateToPathRelativeState(current_distance, state,
+                                                      backwards);
     Eigen::Matrix<double, 5, 1> state_error = relative_goal - relative_state;
     state_error(2, 0) = ::aos::math::NormalizeAngle(state_error(2, 0));
     ::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
 
+    if (backwards) {
+      Eigen::Matrix<double, 2, 1> swapU(U_fb(1), U_fb(0));
+      U_fb = -swapU;
+    }
+
     ::Eigen::Matrix<double, 2, 1> xv_state = current_xva_.block<2, 1>(0, 0);
     next_xva_ = current_trajectory().GetNextXVA(dt_config_.dt, &xv_state);
     next_U_ = U_ff + U_fb - voltage_error;
@@ -184,11 +191,17 @@
     ::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
     trajectory_logging_builder.add_x(goal_state(0));
     trajectory_logging_builder.add_y(goal_state(1));
-    trajectory_logging_builder.add_theta(::aos::math::NormalizeAngle(
-        goal_state(2) +
-        (current_trajectory().drive_spline_backwards() ? M_PI : 0.0)));
-    trajectory_logging_builder.add_left_velocity(goal_state(3));
-    trajectory_logging_builder.add_right_velocity(goal_state(4));
+    if (current_trajectory().drive_spline_backwards()) {
+      trajectory_logging_builder.add_left_velocity(-goal_state(4));
+      trajectory_logging_builder.add_right_velocity(-goal_state(3));
+      trajectory_logging_builder.add_theta(
+          ::aos::math::NormalizeAngle(goal_state(2) + M_PI));
+    } else {
+      trajectory_logging_builder.add_theta(
+          ::aos::math::NormalizeAngle(goal_state(2)));
+      trajectory_logging_builder.add_left_velocity(goal_state(3));
+      trajectory_logging_builder.add_right_velocity(goal_state(4));
+    }
   }
   trajectory_logging_builder.add_is_executing(!IsAtEnd() &&
                                               executing_spline_);
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 08dd3fd..a096fec 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -2,6 +2,7 @@
 
 #include <chrono>
 
+#include "aos/util/math.h"
 #include "Eigen/Dense"
 #include "frc971/control_loops/c2d.h"
 #include "frc971/control_loops/dlqr.h"
@@ -711,7 +712,8 @@
 }
 
 Eigen::Matrix<double, 5, 1> FinishedTrajectory::StateToPathRelativeState(
-    double distance, const Eigen::Matrix<double, 5, 1> &state) const {
+    double distance, const Eigen::Matrix<double, 5, 1> &state,
+    bool drive_backwards) const {
   const double nominal_theta = spline().Theta(distance);
   const Eigen::Matrix<double, 2, 1> nominal_xy = spline().XY(distance);
   const Eigen::Matrix<double, 2, 1> xy_err =
@@ -721,9 +723,16 @@
   Eigen::Matrix<double, 5, 1> path_state;
   path_state(0) = distance + xy_err.x() * ctheta + xy_err.y() * stheta;
   path_state(1) = -xy_err.x() * stheta + xy_err.y() * ctheta;
-  path_state(2) = state(2) - nominal_theta;
+  path_state(2) = aos::math::NormalizeAngle(state(2) - nominal_theta +
+                                            (drive_backwards ? M_PI : 0.0));
+  path_state(2) = aos::math::NormalizeAngle(state(2) - nominal_theta);
   path_state(3) = state(3);
   path_state(4) = state(4);
+  if (drive_backwards) {
+    std::swap(path_state(3), path_state(4));
+    path_state(3) *= -1.0;
+    path_state(4) *= -1.0;
+  }
   return path_state;
 }
 
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index 00b37f6..60695de 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -206,7 +206,8 @@
   // point (i.e., distance should be roughly equal to the actual distance along
   // the path).
   Eigen::Matrix<double, 5, 1> StateToPathRelativeState(
-      double distance, const Eigen::Matrix<double, 5, 1> &state) const;
+      double distance, const Eigen::Matrix<double, 5, 1> &state,
+      bool drive_backwards) const;
 
   // Retrieves the gain matrix K for a given distance along the path.
   Eigen::Matrix<double, 2, 5> GainForDistance(double distance) const;
diff --git a/frc971/control_loops/drivetrain/trajectory_test.cc b/frc971/control_loops/drivetrain/trajectory_test.cc
index c2f6655..c13d844 100644
--- a/frc971/control_loops/drivetrain/trajectory_test.cc
+++ b/frc971/control_loops/drivetrain/trajectory_test.cc
@@ -453,8 +453,8 @@
     const Eigen::Matrix<double, 5, 1> goal_absolute_state =
         finished_trajectory_->GoalState(distance, velocity);
     const Eigen::Matrix<double, 5, 1> goal_relative_state =
-        finished_trajectory_->StateToPathRelativeState(distance,
-                                                       goal_absolute_state);
+        finished_trajectory_->StateToPathRelativeState(
+            distance, goal_absolute_state, false);
     ASSERT_EQ(distance, goal_relative_state(0));
     ASSERT_EQ(0.0, goal_relative_state(1));
     ASSERT_NEAR(goal_absolute_state(2), goal_relative_state(2), 1e-2);
@@ -485,11 +485,11 @@
     const Eigen::Matrix<double, 5, 1> goal_absolute_state =
         finished_trajectory_->GoalState(distance, velocity);
     const Eigen::Matrix<double, 5, 1> goal_relative_state =
-        finished_trajectory_->StateToPathRelativeState(distance,
-                                                       goal_absolute_state);
+        finished_trajectory_->StateToPathRelativeState(
+            distance, goal_absolute_state, false);
     const Eigen::Matrix<double, 5, 1> current_relative_state =
-        finished_trajectory_->StateToPathRelativeState(distance,
-                                                       absolute_state);
+        finished_trajectory_->StateToPathRelativeState(distance, absolute_state,
+                                                       false);
 
     const Eigen::Matrix<double, 2, 1> U_ff =
         finished_trajectory_->FFVoltage(distance);