commit 9a1733a457bd7af66174a37393b7eda83c8c5f63
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Sun Feb 19 16:51:47 2023 -0800
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Mon Feb 20 17:55:34 2023 -0800
tree 2b53ae2fde9fb0044b9e830e63f6763ea8744e08
parent 9f2f53cce6f12359b1d863796156582c0f0d5bfb

Pull some year-generic code out of y2022 localizer

Pull out some utilities that I want to re-use in 2023.

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

y2022/localizer/localizer.h

diff --git a/y2022/localizer/localizer.h b/y2022/localizer/localizer.h
index 917f131..a403ca8 100644
--- a/y2022/localizer/localizer.h
+++ b/y2022/localizer/localizer.h
@@ -12,6 +12,7 @@
 #include "frc971/input/joystick_state_generated.h"
 #include "frc971/control_loops/drivetrain/improved_down_estimator.h"
 #include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "frc971/control_loops/drivetrain/localization_utils.h"
 #include "frc971/zeroing/imu_zeroer.h"
 #include "frc971/zeroing/wrap.h"
 #include "y2022/control_loops/superstructure/superstructure_status_generated.h"
@@ -326,8 +327,6 @@
   ModelBasedLocalizer *localizer() { return &model_based_; }
 
  private:
-  std::optional<aos::monotonic_clock::duration> ClockOffset(
-      std::string_view pi);
   void HandleImu(aos::monotonic_clock::time_point sample_time_pico,
                  aos::monotonic_clock::time_point sample_time_pi,
                  std::optional<Eigen::Vector2d> encoders, Eigen::Vector3d gyro,
@@ -337,21 +336,18 @@
   aos::Sender<LocalizerStatus> status_sender_;
   aos::Sender<LocalizerOutput> output_sender_;
   aos::Sender<LocalizerVisualization> visualization_sender_;
-  aos::Fetcher<frc971::control_loops::drivetrain::Output> output_fetcher_;
-  aos::Fetcher<aos::message_bridge::ServerStatistics> clock_offset_fetcher_;
   std::array<aos::Fetcher<y2022::vision::TargetEstimate>,
              ModelBasedLocalizer::kNumPis>
       target_estimate_fetchers_;
   aos::Fetcher<y2022::control_loops::superstructure::Status>
       superstructure_fetcher_;
-  aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
-  zeroing::ImuZeroer zeroer_;
   aos::monotonic_clock::time_point last_output_send_ =
       aos::monotonic_clock::min_time;
   aos::monotonic_clock::time_point last_visualization_send_ =
       aos::monotonic_clock::min_time;
 
   ImuWatcher imu_watcher_;
+  control_loops::drivetrain::LocalizationUtils utils_;
 };
 }  // namespace frc971::controls
 #endif  // Y2022_LOCALIZER_LOCALIZER_H_