Add feedback to the spline drivetrain.

Change-Id: I11bc55bd6d8ca866dbd85d2b1efcd8e278f8021f
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 88547d5..0f57bf9 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -111,6 +111,15 @@
     return angular;
   }
 
+  Eigen::Matrix<Scalar, 2, 2> Tlr_to_la() const {
+    return (::Eigen::Matrix<Scalar, 2, 2>() << 0.5, 0.5,
+            -1.0 / (2 * robot_radius), 1.0 / (2 * robot_radius)).finished();
+  }
+
+  Eigen::Matrix<Scalar, 2, 2> Tla_to_lr() const {
+    return Tlr_to_la().inverse();
+  }
+
   // Converts the linear and angular position, velocity to the top 4 states of
   // the robot state.
   Eigen::Matrix<Scalar, 4, 1> AngularLinearToLeftRight(