Handle turret wrapping more intelligently in Aimer
This now has actually reasonable logic for handling the turret's full
range of motion. Note that I did have to up the turret range constants a
bit to make it work, since they had just been set to +/-PI previously.
Change-Id: I9c11b56d720cf4aa5064a7f0b9cc5609205b7df7
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index e4742d1..8fb2ca1 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -43,7 +43,11 @@
if (joystick_state_fetcher_.Fetch()) {
alliance = joystick_state_fetcher_->alliance();
}
- aimer_.Update(drivetrain_status_fetcher_.get(), alliance);
+ const turret::Aimer::Mode mode =
+ (unsafe_goal != nullptr && unsafe_goal->shooting())
+ ? turret::Aimer::Mode::kAvoidWrapping
+ : turret::Aimer::Mode::kAvoidEdges;
+ aimer_.Update(drivetrain_status_fetcher_.get(), alliance, mode);
}
const flatbuffers::Offset<AimerStatus> aimer_status_offset =