Tune the spline controller for auto modes
Use the "regular" kalman filter's velocity estimates, since those will
be more stable in the presence of localizer updates.
Change-Id: I9decb651cbdc4a6cf0642ea576c6840e90988803
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 0d100ed..f448760 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -67,9 +67,10 @@
// Returns the current localizer state.
Eigen::Matrix<double, 5, 1> trajectory_state() {
+ // Use the regular kalman filter's left/right velocity because they are
+ // generally smoother.
return (Eigen::Matrix<double, 5, 1>() << localizer_->x(), localizer_->y(),
- localizer_->theta(), localizer_->left_velocity(),
- localizer_->right_velocity())
+ localizer_->theta(), DrivetrainXHat()(1), DrivetrainXHat()(3))
.finished();
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 824f0a8..edda034 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -657,8 +657,8 @@
CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->theta();
// As a sanity check, compare both against absolute angle and the spline's
// goal angle.
- EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), 0.0), 2e-2);
- EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), expected_theta), 2e-2);
+ EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), 0.0), 5e-2);
+ EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), expected_theta), 5e-2);
}
// Tests that simple spline with a single goal message.
@@ -1005,7 +1005,7 @@
const ::Eigen::Vector2d actual = drivetrain_plant_.GetPosition();
// Expect the x position comparison to fail; everything else to succeed.
spline_estimate_tolerance_ = 0.11;
- spline_control_tolerance_ = 0.11;
+ spline_control_tolerance_ = 0.09;
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_);
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 27b31a0..33a8577 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -765,7 +765,7 @@
// TODO(james): Pull these out into a config.
Eigen::Matrix<double, 5, 5> Q;
Q.setIdentity();
- Q.diagonal() << 20.0, 20.0, 20.0, 10.0, 10.0;
+ Q.diagonal() << 30.0, 30.0, 20.0, 15.0, 15.0;
Q *= 2.0;
Q = (Q * Q).eval();