Use ADIS16448 in y2020
This updates the IMU transform to be correct for the yaw axis, but may
have the X/Y axes/signs wrong if we care about accelerometer or
roll/pitch data.
Change-Id: I73d86c8be18f447bb35fc13666ccfdda29fef257
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 2cc9f35..928d418 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -142,21 +142,23 @@
imu_zeroer_.ProcessMeasurements();
got_imu_reading = true;
CHECK(imu_values_fetcher_->has_readings());
- const IMUValues *value = imu_values_fetcher_->readings()->Get(
- imu_values_fetcher_->readings()->size() - 1);
- switch (dt_config_.imu_type) {
- case IMUType::IMU_X:
- last_accel_ = -value->accelerometer_x();
- break;
- case IMUType::IMU_FLIPPED_X:
- last_accel_ = value->accelerometer_x();
- break;
- case IMUType::IMU_Y:
- last_accel_ = -value->accelerometer_y();
- break;
- case IMUType::IMU_Z:
- last_accel_ = value->accelerometer_z();
- break;
+ if (imu_values_fetcher_->readings()->size() > 0) {
+ const IMUValues *value = imu_values_fetcher_->readings()->Get(
+ imu_values_fetcher_->readings()->size() - 1);
+ switch (dt_config_.imu_type) {
+ case IMUType::IMU_X:
+ last_accel_ = -value->accelerometer_x();
+ break;
+ case IMUType::IMU_FLIPPED_X:
+ last_accel_ = value->accelerometer_x();
+ break;
+ case IMUType::IMU_Y:
+ last_accel_ = -value->accelerometer_y();
+ break;
+ case IMUType::IMU_Z:
+ last_accel_ = value->accelerometer_z();
+ break;
+ }
}
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 749b998..d658e21 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -75,8 +75,8 @@
}
// Run for enough time to allow the gyro/imu zeroing code to run.
- RunFor(std::chrono::seconds(10));
- drivetrain_status_fetcher_.Fetch();
+ RunFor(std::chrono::seconds(15));
+ CHECK(drivetrain_status_fetcher_.Fetch());
EXPECT_TRUE(CHECK_NOTNULL(drivetrain_status_fetcher_->zeroing())->zeroed());
}
virtual ~DrivetrainTest() {}
@@ -91,10 +91,10 @@
drivetrain_plant_.GetRightPosition(), 1e-2);
}
- void VerifyNearPosition(double x, double y) {
+ void VerifyNearPosition(double x, double y, double eps = 1e-2) {
auto actual = drivetrain_plant_.GetPosition();
- EXPECT_NEAR(actual(0), x, 1e-2);
- EXPECT_NEAR(actual(1), y, 1e-2);
+ EXPECT_NEAR(actual(0), x, eps);
+ EXPECT_NEAR(actual(1), y, eps);
}
void VerifyNearSplineGoal() {
@@ -214,6 +214,9 @@
// Fault the IMU and confirm that we disable the outputs.
drivetrain_plant_.set_imu_faulted(true);
+ // Ensure the fault has time to propagate.
+ RunFor(2 * dt());
+
for (int i = 0; i < 500; ++i) {
RunFor(dt());
ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
@@ -1261,7 +1264,7 @@
WaitForTrajectoryExecution();
VerifyNearSplineGoal();
- VerifyNearPosition(1.0, 2.0);
+ VerifyNearPosition(1.0, 2.0, 5e-2);
}
// Tests that splines can excecute and plan at the same time.
@@ -1413,7 +1416,7 @@
}
WaitForTrajectoryExecution();
- VerifyNearPosition(1.0, 1.0);
+ VerifyNearPosition(1.0, 1.0, 5e-2);
}
// Tests that when we send a bunch of splines we properly evict old splines from
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 3fa138c..aabd1c6 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -148,7 +148,7 @@
dt_config_.dt);
// TODO(milind): We should be able to get IMU readings at 1 kHz instead of 2.
event_loop_->AddPhasedLoop([this](int) { ReadImu(); },
- std::chrono::microseconds(500));
+ std::chrono::milliseconds(5));
}
void DrivetrainSimulation::Reinitialize() {
diff --git a/frc971/zeroing/imu_zeroer.h b/frc971/zeroing/imu_zeroer.h
index fd82571..2473a66 100644
--- a/frc971/zeroing/imu_zeroer.h
+++ b/frc971/zeroing/imu_zeroer.h
@@ -16,7 +16,7 @@
// constant number of samples...
// TODO(james): Run average and GetRange calculations over every sample on
// every timestep, to provide consistent timing.
- static constexpr size_t kSamplesToAverage = 1000;
+ static constexpr size_t kSamplesToAverage = 200;
static constexpr size_t kRequiredZeroPoints = 10;
ImuZeroer();