Add pivot joint functionality
Signed-off-by: Charlie Huang <charliehuang09@gmail.com>
Change-Id: Ib40d4ce1a4c91f1b0e75b3548fe43c9218433634
diff --git a/y2023_bot3/wpilib_interface.cc b/y2023_bot3/wpilib_interface.cc
index 09f87db..2dd1144 100644
--- a/y2023_bot3/wpilib_interface.cc
+++ b/y2023_bot3/wpilib_interface.cc
@@ -92,6 +92,7 @@
constexpr double kMaxFastEncoderPulsesPerSecond = std::max({
Values::kMaxDrivetrainEncoderPulsesPerSecond(),
+ Values::kMaxPivotJointEncoderPulsesPerSecond(),
});
static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
"fast encoders are too fast");
@@ -426,11 +427,11 @@
roller_falcon_offset = frc971::control_loops::CANFalcon::Pack(
*builder.fbb(), &optional_roller_falcon.value());
}
-
superstructure::Position::Builder position_builder =
builder.MakeBuilder<superstructure::Position>();
position_builder.add_end_effector_cube_beam_break(
!end_effector_cube_beam_break_->Get());
+
if (!roller_falcon_offset.IsNull()) {
position_builder.add_roller_falcon(roller_falcon_offset);
}