commit f6aa038efb5d70db0d6c4b9d9079219eab82a8ee
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Fri Mar 01 19:46:05 2024 -0800
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Fri Mar 01 21:42:14 2024 -0800
tree 1524924db59872f3fe6dd848993641c75e6b390e
parent 0b341f566a5842055171de184812b3cc55fa96ed

Swap to combined voltage/encoder message for localizer

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

y2024/localizer/localizer.cc

diff --git a/y2024/localizer/localizer.cc b/y2024/localizer/localizer.cc
index 441360c..a177188 100644
--- a/y2024/localizer/localizer.cc
+++ b/y2024/localizer/localizer.cc
@@ -406,11 +406,7 @@
   // the camera measurement and the current estimate of the
   // pose. This doesn't affect any of the math, it just makes the code a bit
   // more convenient to write given the Correct() interface we already have.
-  observations_.CorrectKnownH(
-      Eigen::Vector3d::Zero(), &U,
-      Corrector(state_at_capture.value(), H_field_target, H_robot_camera,
-                H_camera_target),
-      R, t_);
+  observations_.CorrectKnownH(Eigen::Vector3d::Zero(), &U, corrector, R, t_);
   ++total_accepted_targets_;
   ++cameras_.at(camera_index).total_accepted_targets;
   VLOG(1) << "new state " << ekf_.X_hat().topRows<3>().transpose();

y2024/localizer/localizer_test.cc

diff --git a/y2024/localizer/localizer_test.cc b/y2024/localizer/localizer_test.cc
index 25c0bab..a2fc200 100644
--- a/y2024/localizer/localizer_test.cc
+++ b/y2024/localizer/localizer_test.cc
@@ -6,6 +6,7 @@
 #include "aos/events/simulated_event_loop.h"
 #include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
 #include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "frc971/control_loops/drivetrain/rio_localizer_inputs_static.h"
 #include "frc971/control_loops/pose.h"
 #include "frc971/vision/target_map_generated.h"
 #include "frc971/vision/target_map_utils.h"
@@ -60,6 +61,10 @@
         constants_fetcher_(imu_test_event_loop_.get()),
         output_sender_(
             roborio_test_event_loop_->MakeSender<Output>("/drivetrain")),
+        combined_sender_(
+            roborio_test_event_loop_->MakeSender<
+                frc971::control_loops::drivetrain::RioLocalizerInputsStatic>(
+                "/drivetrain")),
         target_sender_(
             camera_test_event_loop_->MakeSender<frc971::vision::TargetMap>(
                 "/camera0")),
@@ -81,6 +86,14 @@
           output_builder.add_right_voltage(output_voltages_(1));
           builder.CheckOk(builder.Send(output_builder.Finish()));
         }
+        {
+          auto builder = combined_sender_.MakeStaticBuilder();
+          builder->set_left_voltage(output_voltages_(0));
+          builder->set_right_voltage(output_voltages_(1));
+          builder->set_left_encoder(drivetrain_plant_.GetLeftPosition());
+          builder->set_right_encoder(drivetrain_plant_.GetRightPosition());
+          builder.CheckOk(builder.Send());
+        }
       });
       roborio_test_event_loop_->OnRun([timer, this]() {
         timer->Schedule(roborio_test_event_loop_->monotonic_now(),
@@ -274,6 +287,8 @@
   frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
 
   aos::Sender<Output> output_sender_;
+  aos::Sender<frc971::control_loops::drivetrain::RioLocalizerInputsStatic>
+      combined_sender_;
   aos::Sender<frc971::vision::TargetMap> target_sender_;
   aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
       control_sender_;