commit 207ae32a45acb2ac208efcfe31074714f4892bde
author James Kuszmaul <jabukuszmaul@gmail.com> Fri Feb 25 21:15:31 2022 -0800
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Fri Feb 25 23:42:20 2022 -0800
tree 57a9d7d986f7697530eedf0a3571f1064dc97d6b
parent e2f40d7d59c43b2c827a39569930810af66e8850

Tune down estimator for ADIS16505

* Update down_estimator_plotter for the ADIS16505 setup (would be
  nice to refactor things to make it so that you can apply a plot
  config to a particular submessage type...).
* Crank up the threshold to consider the IMU still, to avoid resetting
  in the middle of real motion.
* Also check in the IMU transform from the jankily-mounted IMU on the
  2020 robot.

Change-Id: I4c6791524c3436614db045d3f2ba371f2400b084
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>

y2022/control_loops/drivetrain/drivetrain_base.cc

diff --git a/y2022/control_loops/drivetrain/drivetrain_base.cc b/y2022/control_loops/drivetrain/drivetrain_base.cc
index 93233d2..1a76afc 100644
--- a/y2022/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2022/control_loops/drivetrain/drivetrain_base.cc
@@ -10,6 +10,7 @@
 #include "y2022/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
 
 using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+using ::frc971::control_loops::drivetrain::DownEstimatorConfig;
 
 namespace chrono = ::std::chrono;
 
@@ -22,6 +23,8 @@
 const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
 
 const DrivetrainConfig<double> &GetDrivetrainConfig() {
+  // Yaw of the IMU relative to the robot frame.
+  static constexpr double kImuYaw = -M_PI_2 + 0.15;
   static DrivetrainConfig<double> kDrivetrainConfig{
       ::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
       ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
@@ -53,7 +56,12 @@
       1.2 /* quickturn_wheel_multiplier */,
       1.2 /* wheel_multiplier */,
       true /*pistol_grip_shift_enables_line_follow*/,
-  };
+      (Eigen::Matrix<double, 3, 3>() << std::cos(kImuYaw), -std::sin(kImuYaw),
+       0.0, std::sin(kImuYaw), std::cos(kImuYaw), 0.0, 0.0, 0.0, 1.0)
+          .finished(),
+      false /*is_simulated*/,
+      DownEstimatorConfig{.gravity_threshold = 0.015,
+                          .do_accel_corrections = 1000}};
 
   return kDrivetrainConfig;
 };