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.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index b6b8b5d..cab8043 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -232,11 +232,9 @@
Y << position->left_encoder, position->right_encoder, last_gyro_rate_,
last_accel_;
kf_.Correct(Y);
- // TODO(james): Account for delayed_U as appropriate (should be
- // last_last_*_voltage).
- localizer_->Update({last_left_voltage_, last_right_voltage_}, monotonic_now,
- position->left_encoder, position->right_encoder,
- last_gyro_rate_, last_accel_);
+ localizer_->Update({last_last_left_voltage_, last_last_right_voltage_},
+ monotonic_now, position->left_encoder,
+ position->right_encoder, last_gyro_rate_, last_accel_);
}
dt_openloop_.SetPosition(position, left_gear_, right_gear_);
@@ -348,6 +346,8 @@
// Gyro heading vs left-right
// Voltage error.
+ last_last_left_voltage_ = last_left_voltage_;
+ last_last_right_voltage_ = last_right_voltage_;
Eigen::Matrix<double, 2, 1> U;
U(0, 0) = last_left_voltage_;
U(1, 0) = last_right_voltage_;
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_;