Stop nesting the builder for the superstructure
Also read the flywheel last. That makes the timestamps better.
Change-Id: I5f5c9bf4963c88d203884e1ced530a4dc511f5a3
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index 4275506..4f39acf 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -218,8 +218,7 @@
{
auto builder = superstructure_position_sender_.MakeBuilder();
- superstructure::Position::Builder position_builder =
- builder.MakeBuilder<superstructure::Position>();
+
// TODO(alex): check new absolute encoder api.
// Hood
frc971::AbsolutePositionT hood;
@@ -246,6 +245,14 @@
flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &turret);
+ // Control Panel
+ frc971::RelativePositionT control_panel;
+ CopyPosition(*control_panel_encoder_, &control_panel,
+ Values::kControlPanelEncoderCountsPerRevolution(),
+ Values::kControlPanelEncoderRatio(), false);
+ flatbuffers::Offset<frc971::RelativePosition> control_panel_offset =
+ frc971::RelativePosition::Pack(*builder.fbb(), &control_panel);
+
// Shooter
y2020::control_loops::superstructure::ShooterPositionT shooter;
shooter.theta_finisher =
@@ -266,14 +273,8 @@
y2020::control_loops::superstructure::ShooterPosition::Pack(
*builder.fbb(), &shooter);
- // Control Panel
- frc971::RelativePositionT control_panel;
- CopyPosition(*control_panel_encoder_, &control_panel,
- Values::kControlPanelEncoderCountsPerRevolution(),
- Values::kControlPanelEncoderRatio(), false);
- flatbuffers::Offset<frc971::RelativePosition> control_panel_offset =
- frc971::RelativePosition::Pack(*builder.fbb(), &control_panel);
-
+ superstructure::Position::Builder position_builder =
+ builder.MakeBuilder<superstructure::Position>();
position_builder.add_hood(hood_offset);
position_builder.add_intake_joint(intake_joint_offset);
position_builder.add_turret(turret_offset);