Tweaked collision avoidance to tuck in more.
Change-Id: I8aa5c43d74ef1cbff0543f33284ea2661e13ab76
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 1f1bbdf..b82d2d9 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -56,14 +56,22 @@
// Make sure that we don't move the shoulder below a certain angle until
// the wrist is level with the ground.
- if (::std::abs(wrist_angle) > kMaxWristAngleForMovingByIntake) {
- shoulder_angle_goal =
+ if (intake_angle < kMaxIntakeAngleBeforeArmInterference + kSafetyMargin) {
+ if (::std::abs(wrist_angle) > kMaxWristAngleForMovingByIntake) {
+ shoulder_angle_goal =
::std::max(original_shoulder_angle_goal,
- kMinShoulderAngleForIntakeUpInterference + kSafetyMargin);
+ kMinShoulderAngleForIntakeInterference + kSafetyMargin);
+ }
+ } else {
+ if (::std::abs(wrist_angle) > kMaxWristAngleForMovingByIntake) {
+ shoulder_angle_goal =
+ ::std::max(original_shoulder_angle_goal,
+ kMinShoulderAngleForIntakeUpInterference + kSafetyMargin);
+ }
}
if (::std::abs(wrist_angle) > kMaxWristAngleForSafeArmStowing) {
shoulder_angle_goal =
- ::std::max(original_shoulder_angle_goal,
+ ::std::max(shoulder_angle_goal,
kMinShoulderAngleForHorizontalShooter + kSafetyMargin);
}
}