Disable voltage error compensation for spline tracking.
The current voltage error terms are behaving unexpectedly, so disable
the compensation until we get a chance to debug further.
Change-Id: I6807fc90bea65e27fdae71ffeeeb54b94e45048c
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 8c0599f..88e7a02 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -297,9 +297,16 @@
localizer_->right_velocity())
.finished();
- dt_spline_.Update(
- output != nullptr && controller_type == ControllerType::SPLINE_FOLLOWER,
- trajectory_state, kf_.X_hat().block<2, 1>(4, 0));
+ {
+ // TODO(james): The regular Kalman Filter's voltage error terms are
+ // currently unusable--either don't use voltage error at all for the spline
+ // following code, or use the EKF's voltage error estimates.
+ const Eigen::Matrix<double, 2, 1> voltage_error =
+ 0 * kf_.X_hat().block<2, 1>(4, 0);
+ dt_spline_.Update(
+ output != nullptr && controller_type == ControllerType::SPLINE_FOLLOWER,
+ trajectory_state, voltage_error);
+ }
dt_line_follow_.Update(monotonic_now, trajectory_state);