commit 3e1bb2742bd3dc64be02e4416e2af1c59c8ab493
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Fri Jan 17 18:38:19 2020 -0800
committer James Kuszmaul <jabukuszmaul@gmail.com> Sun Jan 19 17:53:15 2020 -0800
tree 83d9ce8b42860a54825f23b332119b92e8493a60
parent 17d83cb2217f2bdd6f73bd1e2e639f555bc64259

Actually run down estimator in drivetrain

Change-Id: I8602c448b632e966fe3f252b48a0f7ec0726285e

frc971/control_loops/drivetrain/drivetrain.h

diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 9f6a34c..f7924c5 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -13,6 +13,7 @@
 #include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
 #include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #include "frc971/control_loops/drivetrain/gear.h"
+#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
 #include "frc971/control_loops/drivetrain/line_follow_drivetrain.h"
 #include "frc971/control_loops/drivetrain/localizer.h"
 #include "frc971/control_loops/drivetrain/localizer_generated.h"
@@ -21,6 +22,7 @@
 #include "frc971/control_loops/drivetrain/ssdrivetrain.h"
 #include "frc971/queues/gyro_generated.h"
 #include "frc971/wpilib/imu_generated.h"
+#include "frc971/zeroing/imu_zeroer.h"
 
 namespace frc971 {
 namespace control_loops {
@@ -57,6 +59,11 @@
   ::aos::Fetcher<LocalizerControl> localizer_control_fetcher_;
   ::aos::Fetcher<::frc971::IMUValues> imu_values_fetcher_;
   ::aos::Fetcher<::frc971::sensors::GyroReading> gyro_reading_fetcher_;
+
+  zeroing::ImuZeroer imu_zeroer_;
+  DrivetrainUkf down_estimator_;
+  aos::monotonic_clock::time_point last_imu_update_ =
+      aos::monotonic_clock::min_time;
   LocalizerInterface *localizer_;
 
   StateFeedbackLoop<7, 2, 4> kf_;
@@ -67,9 +74,6 @@
   ::aos::monotonic_clock::time_point last_gyro_time_ =
       ::aos::monotonic_clock::min_time;
 
-  StateFeedbackLoop<2, 1, 1> down_estimator_;
-  Eigen::Matrix<double, 1, 1> down_U_;
-
   // Current gears for each drive side.
   Gear left_gear_;
   Gear right_gear_;