Fix backwards spline feedback
We weren't handling the logic for feedback on backwards splines
correctly.
TODO: Clean up some of the backwards logic to make it possible to
follow.
Change-Id: I0c18b8c245010151356935dfcbf8babdb98d0846
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;