Swap to combined voltage/encoder message for localizer
Change-Id: I23b553b142bde46c321409105d49a716eb7059b6
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2024/BUILD b/y2024/BUILD
index 9e27ab2..212f70e 100644
--- a/y2024/BUILD
+++ b/y2024/BUILD
@@ -154,6 +154,7 @@
"//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
"//y2024/control_loops/superstructure:superstructure_can_position_fbs",
"//y2024/control_loops/superstructure:superstructure_output_fbs",
+ "//frc971/control_loops/drivetrain:rio_localizer_inputs_fbs",
"//y2024/control_loops/superstructure:superstructure_position_fbs",
"//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//y2024/control_loops/superstructure:superstructure_status_fbs",
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();
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_;
diff --git a/y2024/y2024_roborio.json b/y2024/y2024_roborio.json
index 2f5f4b7..5ba882a 100644
--- a/y2024/y2024_roborio.json
+++ b/y2024/y2024_roborio.json
@@ -222,6 +222,14 @@
"source_node": "roborio",
"frequency": 400,
"max_size": 128,
+ "num_senders": 2
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.RioLocalizerInputs",
+ "source_node": "roborio",
+ "frequency": 400,
+ "max_size": 80,
"num_senders": 2,
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
@@ -241,18 +249,7 @@
"source_node": "roborio",
"frequency": 400,
"max_size": 80,
- "num_senders": 2,
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_nodes": [
- "imu"
- ],
- "destination_nodes": [
- {
- "name": "imu",
- "priority": 5,
- "time_to_live": 5000000
- }
- ]
+ "num_senders": 2
},
{
"name": "/drivetrain",