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(