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/turret/aiming.h b/y2020/control_loops/superstructure/turret/aiming.h
index 675eea3..5fbbdc6 100644
--- a/y2020/control_loops/superstructure/turret/aiming.h
+++ b/y2020/control_loops/superstructure/turret/aiming.h
@@ -26,8 +26,20 @@
typedef frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
Goal;
typedef frc971::control_loops::drivetrain::Status Status;
+ // Mode to run the aimer in, to control how we manage wrapping the turret
+ // angle.
+ enum class Mode {
+ // Keep the turret as far away from the edges of the range of motion as
+ // reasonable, to minimize the odds that we will hit the hardstops once we
+ // start shooting.
+ kAvoidEdges,
+ // Do everything reasonable to avoid having to wrap the shooter--set this
+ // while shooting so that we don't randomly spin the shooter 360 while
+ // shooting.
+ kAvoidWrapping,
+ };
Aimer();
- void Update(const Status *status, aos::Alliance alliance);
+ void Update(const Status *status, aos::Alliance alliance, Mode mode);
const Goal *TurretGoal() const { return &goal_.message(); }
// Returns the distance to the goal, in meters.
double DistanceToGoal() const { return distance_; }