Make drivetrain_test_lib handle IMU transforms
This also updates the 2020 imu transform to better match the 2020
drivetrain. I haven't actually checked the yaw orientation of the IMU
though.
Change-Id: Ib036705269dfc467a60714c456751a6cfca8a67e
diff --git a/y2020/control_loops/drivetrain/drivetrain_base.cc b/y2020/control_loops/drivetrain/drivetrain_base.cc
index 62679bc..b919902 100644
--- a/y2020/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_base.cc
@@ -25,8 +25,8 @@
static DrivetrainConfig<double> kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
- ::frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO,
- ::frc971::control_loops::drivetrain::IMUType::IMU_FLIPPED_X,
+ ::frc971::control_loops::drivetrain::GyroType::IMU_X_GYRO,
+ ::frc971::control_loops::drivetrain::IMUType::IMU_Z,
drivetrain::MakeDrivetrainLoop,
drivetrain::MakeVelocityDrivetrainLoop,
@@ -53,8 +53,8 @@
1.2 /* quickturn_wheel_multiplier */,
1.2 /* wheel_multiplier */,
true /*pistol_grip_shift_enables_line_follow*/,
- (Eigen::Matrix<double, 3, 3>() << 0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
- 1.0)
+ (Eigen::Matrix<double, 3, 3>() << 0.0, 0.0, 1.0, 0.0, -1.0, 0.0, 1.0, 0.0,
+ 0.0)
.finished() /*imu_transform*/,
};
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay_test.cc b/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
index 3e391c8..6033184 100644
--- a/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
@@ -52,13 +52,19 @@
reader_.event_loop_factory()->MakeEventLoop("drivetrain", roborio_);
drivetrain_event_loop_->SkipTimingReport();
+ frc971::control_loops::drivetrain::DrivetrainConfig<double> config =
+ GetDrivetrainConfig();
+ // Make the modification required to the imu transform to work with the 2016
+ // logs...
+ config.imu_transform << 0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0;
+ config.gyro_type = frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO;
+
localizer_ =
std::make_unique<frc971::control_loops::drivetrain::DeadReckonEkf>(
- drivetrain_event_loop_.get(), GetDrivetrainConfig());
+ drivetrain_event_loop_.get(), config);
drivetrain_ =
std::make_unique<frc971::control_loops::drivetrain::DrivetrainLoop>(
- GetDrivetrainConfig(), drivetrain_event_loop_.get(),
- localizer_.get());
+ config, drivetrain_event_loop_.get(), localizer_.get());
test_event_loop_ =
reader_.event_loop_factory()->MakeEventLoop("drivetrain", roborio_);
@@ -88,7 +94,7 @@
ASSERT_TRUE(status_fetcher_->has_x());
ASSERT_TRUE(status_fetcher_->has_y());
ASSERT_TRUE(status_fetcher_->has_theta());
- EXPECT_LT(std::abs(status_fetcher_->x()), 0.1);
+ EXPECT_LT(std::abs(status_fetcher_->x()), 0.25);
// Because the encoders should not be affecting the y or yaw axes, expect a
// reasonably precise result (although, since this is a real worl dtest, the
// robot probably did actually move be some non-zero amount).
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index c438af5..c47faa7 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -92,7 +92,6 @@
namespace chrono = std::chrono;
using frc971::control_loops::drivetrain::testing::DrivetrainSimulation;
using frc971::control_loops::drivetrain::DrivetrainLoop;
-using frc971::control_loops::drivetrain::testing::GetTestDrivetrainConfig;
using aos::monotonic_clock;
class LocalizedDrivetrainTest : public aos::testing::ControlLoopTest {
@@ -418,6 +417,22 @@
EXPECT_TRUE(VerifyEstimatorAccurate(5e-4));
}
+// Tests that we can drive in a straight line and have things estimate
+// correctly.
+TEST_F(LocalizedDrivetrainTest, NoCameraUpdateStraightLine) {
+ SetEnabled(true);
+ set_enable_cameras(false);
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-7));
+
+ SendGoal(1.0, 1.0);
+
+ RunFor(chrono::seconds(1));
+ VerifyNearGoal();
+ // Due to accelerometer drift, the straight-line driving tends to be less
+ // accurate...
+ EXPECT_TRUE(VerifyEstimatorAccurate(0.1));
+}
+
// Tests that camera updates with a perfect models results in no errors.
TEST_F(LocalizedDrivetrainTest, PerfectCameraUpdate) {
SetEnabled(true);