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);
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 9857196..bfa3d26 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -901,6 +901,8 @@
// Tests that simple spline converges when we introduce a straight voltage
// error.
+// TODO(james): Reenable this once we decide what to do with the voltage error
+// terms.
TEST_F(DrivetrainTest, SplineVoltageError) {
SetEnabled(true);
drivetrain_plant_.set_left_voltage_offset(1.0);
@@ -947,7 +949,21 @@
WaitForTrajectoryPlan();
RunFor(chrono::milliseconds(5000));
- VerifyNearSplineGoal();
+ // Since the voltage error compensation is disabled, expect that we will have
+ // *failed* to reach our goal.
+ drivetrain_status_fetcher_.Fetch();
+ const double expected_x =
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x();
+ const double expected_y =
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y();
+ const double estimated_x = drivetrain_status_fetcher_->x();
+ const double estimated_y = drivetrain_status_fetcher_->y();
+ const ::Eigen::Vector2d actual = drivetrain_plant_.GetPosition();
+ // Expect the x position comparison to fail; everything else to succeed.
+ EXPECT_GT(std::abs(estimated_x - expected_x), spline_control_tolerance_);
+ EXPECT_NEAR(estimated_y, expected_y, spline_control_tolerance_);
+ EXPECT_NEAR(actual(0), estimated_x, spline_estimate_tolerance_);
+ EXPECT_NEAR(actual(1), estimated_y, spline_estimate_tolerance_);
}
// Tests that a multispline converges on a goal.