Avoid lifting the elevator on overshoot of the wrist
We were overshooting, hitting the forward limit, pushing the elevator up
to clear, then repeating. Just give ourselves a bit more space.
Change-Id: I6baa2c4e2b1454c8381588bad9081de5bdc272fe
diff --git a/y2019/control_loops/superstructure/collision_avoidance.h b/y2019/control_loops/superstructure/collision_avoidance.h
index 90a45f6..364836a 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.h
+++ b/y2019/control_loops/superstructure/collision_avoidance.h
@@ -59,7 +59,7 @@
static constexpr double kElevatorClearIntakeHeight = 0.4;
// Angle constraints for the wrist when below kElevatorClearDownHeight
- static constexpr double kWristMaxAngle = M_PI / 2.0 + 0.05;
+ static constexpr double kWristMaxAngle = M_PI / 2.0 + 0.15;
static constexpr double kWristMinAngle = -M_PI / 2.0 - 0.25;
// Angles outside of which the intake is fully clear of the wrist.