Add getters for x, y, theta in autonomous
Super handy for triggering behavior based on position.
Change-Id: I435617ffc786a48f51c7ed1c2cf75790300d2201
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index ca6cc89..492302f 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -177,6 +177,24 @@
return false;
}
+double BaseAutonomousActor::X() {
+ drivetrain_status_fetcher_.Fetch();
+ CHECK(drivetrain_status_fetcher_.get());
+ return drivetrain_status_fetcher_->x();
+}
+
+double BaseAutonomousActor::Y() {
+ drivetrain_status_fetcher_.Fetch();
+ CHECK(drivetrain_status_fetcher_.get());
+ return drivetrain_status_fetcher_->y();
+}
+
+double BaseAutonomousActor::Theta() {
+ drivetrain_status_fetcher_.Fetch();
+ CHECK(drivetrain_status_fetcher_.get());
+ return drivetrain_status_fetcher_->theta();
+}
+
bool BaseAutonomousActor::WaitForAboveAngle(double angle) {
::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),