Account for delayed U in using localizer
This makes the tests pass to within numerical precision errors, rather
than to merely very small errors.
Change-Id: I3e764082e3f86c2d06fc9e7a3bbc6ef749a75d85
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 35d6084..800199b 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -64,6 +64,9 @@
double last_left_voltage_ = 0;
double last_right_voltage_ = 0;
+ // The left/right voltages previous to last_*_voltage_.
+ double last_last_left_voltage_ = 0;
+ double last_last_right_voltage_ = 0;
bool left_high_requested_;
bool right_high_requested_;