Add targets for the turret aiming to shoot at
This adds a basic aiming algorithm that aims at the 3-point shot when we
think it can make it, and a 2-point shot otherwise.
Note: This also formalizes the fact that we are using the origin that is
defined by sift.fbs this year.
Change-Id: I04de0d01e2596bfa75226c6ac00af1d1d08d487f
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index d1731e1..e4742d1 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -18,7 +18,9 @@
turret_(constants::GetValues().turret.subsystem_params),
drivetrain_status_fetcher_(
event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
- "/drivetrain")) {
+ "/drivetrain")),
+ joystick_state_fetcher_(
+ event_loop->MakeFetcher<aos::JoystickState>("/aos")) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -37,7 +39,11 @@
event_loop()->context().monotonic_event_time;
if (drivetrain_status_fetcher_.Fetch()) {
- aimer_.Update(drivetrain_status_fetcher_.get());
+ aos::Alliance alliance = aos::Alliance::kInvalid;
+ if (joystick_state_fetcher_.Fetch()) {
+ alliance = joystick_state_fetcher_->alliance();
+ }
+ aimer_.Update(drivetrain_status_fetcher_.get(), alliance);
}
const flatbuffers::Offset<AimerStatus> aimer_status_offset =