Swap to combined voltage/encoder message for localizer
Change-Id: I23b553b142bde46c321409105d49a716eb7059b6
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 2b84a9d..95fab0a2 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -350,7 +350,11 @@
dt_openloop_(dt_config_, filters_.kf()),
dt_closedloop_(dt_config_, filters_.kf(), localizer),
dt_spline_(dt_config_),
- dt_line_follow_(dt_config_, localizer->target_selector()) {
+ dt_line_follow_(dt_config_, localizer->target_selector()),
+ localizer_input_sender_(
+ event_loop->TryMakeSender<
+ frc971::control_loops::drivetrain::RioLocalizerInputsStatic>(
+ "/drivetrain")) {
event_loop->SetRuntimeRealtimePriority(30);
for (size_t ii = 0; ii < trajectory_fetchers_.size(); ++ii) {
trajectory_fetchers_[ii].fetcher =
@@ -613,6 +617,17 @@
if (output) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
+
+ if (localizer_input_sender_.valid()) {
+ auto localizer_input_builder = localizer_input_sender_.MakeStaticBuilder();
+ localizer_input_builder->set_left_encoder(position->left_encoder());
+ localizer_input_builder->set_right_encoder(position->right_encoder());
+ if (output) {
+ localizer_input_builder->set_left_voltage(output_struct.left_voltage);
+ localizer_input_builder->set_right_voltage(output_struct.right_voltage);
+ }
+ localizer_input_builder.CheckOk(localizer_input_builder.Send());
+ }
}
flatbuffers::Offset<Output> DrivetrainLoop::Zero(