Tune the localizer to work reasonably well
The biggest changes here are adding back in voltage/angular error (in
effect, reverting parts of I9744c4808edf3a43ae1c76d022460ee1d4c9ed3e)
and tweaking some of the constants so that we better trust encoders.
Change-Id: Ibd9488e0d92d86792bb7cc6437a387589252a463
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 0015b70..6a2ea76 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -242,7 +242,8 @@
// TODO(james): Use a watcher (instead of a fetcher) once we support it in
// simulation.
if (localizer_control_fetcher_.Fetch()) {
- localizer_->ResetPosition(localizer_control_fetcher_->x,
+ LOG_STRUCT(DEBUG, "localizer_control", *localizer_control_fetcher_);
+ localizer_->ResetPosition(monotonic_now, localizer_control_fetcher_->x,
localizer_control_fetcher_->y,
localizer_control_fetcher_->theta);
}