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_;
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index bcbad52..d491c14 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -76,7 +76,6 @@
void RunIteration() {
drivetrain_motor_plant_.SendPositionMessage();
drivetrain_motor_.Iterate();
- drivetrain_motor_plant_.Simulate();
if (enable_cameras_) {
SendDelayedFrames();
if (last_frame_ + ::std::chrono::milliseconds(33) <
@@ -85,6 +84,7 @@
last_frame_ = monotonic_clock::now();
}
}
+ drivetrain_motor_plant_.Simulate();
SimulateTimestep(true, dt_config_.dt);
}
@@ -107,14 +107,14 @@
drivetrain_motor_plant_.GetRightPosition(), 1e-3);
}
- void VerifyEstimatorAccurate() {
+ void VerifyEstimatorAccurate(double eps) {
const Eigen::Matrix<double, 5, 1> true_state =
drivetrain_motor_plant_.state();
- EXPECT_NEAR(localizer_.x(), true_state(0, 0), 1e-5);
- EXPECT_NEAR(localizer_.y(), true_state(1, 0), 1e-5);
- EXPECT_NEAR(localizer_.theta(), true_state(2, 0), 1e-5);
- EXPECT_NEAR(localizer_.left_velocity(), true_state(3, 0), 1e-5);
- EXPECT_NEAR(localizer_.right_velocity(), true_state(4, 0), 1e-5);
+ EXPECT_NEAR(localizer_.x(), true_state(0, 0), eps);
+ EXPECT_NEAR(localizer_.y(), true_state(1, 0), eps);
+ EXPECT_NEAR(localizer_.theta(), true_state(2, 0), eps);
+ EXPECT_NEAR(localizer_.left_velocity(), true_state(3, 0), eps);
+ EXPECT_NEAR(localizer_.right_velocity(), true_state(4, 0), eps);
}
void CaptureFrames() {
@@ -199,7 +199,20 @@
.Send();
RunForTime(chrono::seconds(3));
VerifyNearGoal();
- VerifyEstimatorAccurate();
+ VerifyEstimatorAccurate(1e-10);
+}
+
+// Tests that camera udpates with a perfect models results in no errors.
+TEST_F(LocalizedDrivetrainTest, PerfectCameraUpdate) {
+ set_enable_cameras(true);
+ my_drivetrain_queue_.goal.MakeWithBuilder()
+ .controller_type(1)
+ .left_goal(-1.0)
+ .right_goal(1.0)
+ .Send();
+ RunForTime(chrono::seconds(3));
+ VerifyNearGoal();
+ VerifyEstimatorAccurate(1e-8);
}
// Tests that not having cameras with an initial disturbance results in
@@ -237,7 +250,7 @@
.Send();
RunForTime(chrono::seconds(3));
VerifyNearGoal();
- VerifyEstimatorAccurate();
+ VerifyEstimatorAccurate(1e-5);
}
} // namespace testing