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(),
diff --git a/frc971/autonomous/base_autonomous_actor.h b/frc971/autonomous/base_autonomous_actor.h
index a70a440..5562dde 100644
--- a/frc971/autonomous/base_autonomous_actor.h
+++ b/frc971/autonomous/base_autonomous_actor.h
@@ -91,6 +91,11 @@
// Returns true if the drive has finished.
bool IsDriveDone();
+ // Returns the current x, y, theta of the robot on the field.
+ double X();
+ double Y();
+ double Theta();
+
void LineFollowAtVelocity(
double velocity,
y2019::control_loops::drivetrain::SelectionHint hint =