Add more functionality to year-agnostic localization utils
* Allow retrieving encoder values from the roborio.
* Move a utility for calculating some H matrices from y2019
into the year-agnostic code.
Change-Id: I2ef0e391fc615ccceeb2b0fc035233bb995d6e1e
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/drivetrain/localization/utils.cc b/frc971/control_loops/drivetrain/localization/utils.cc
index 10ee445..fb46219 100644
--- a/frc971/control_loops/drivetrain/localization/utils.cc
+++ b/frc971/control_loops/drivetrain/localization/utils.cc
@@ -4,6 +4,7 @@
LocalizationUtils::LocalizationUtils(aos::EventLoop *event_loop)
: output_fetcher_(event_loop->MakeFetcher<Output>("/drivetrain")),
+ position_fetcher_(event_loop->TryMakeFetcher<Position>("/drivetrain")),
clock_offset_fetcher_(
event_loop->MakeFetcher<aos::message_bridge::ServerStatistics>(
"/aos")),
@@ -23,6 +24,20 @@
output_fetcher_->right_voltage()};
}
+std::optional<Eigen::Vector2d> LocalizationUtils::Encoders(
+ aos::monotonic_clock::time_point now) {
+ CHECK(position_fetcher_.valid());
+ position_fetcher_.Fetch();
+ const bool stale = (position_fetcher_.get() == nullptr) ||
+ (position_fetcher_.context().monotonic_event_time +
+ std::chrono::milliseconds(10) <
+ now);
+ return stale ? std::nullopt
+ : std::make_optional<Eigen::Vector2d>(
+ position_fetcher_->left_encoder(),
+ position_fetcher_->right_encoder());
+}
+
bool LocalizationUtils::MaybeInAutonomous() {
joystick_state_fetcher_.Fetch();
return (joystick_state_fetcher_.get() != nullptr)