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);
}
}
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 74a059d..fd00fd1 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -88,7 +88,7 @@
// The maximum absolute angle in radians that the wrist can be from horizontal
// while it is near the intake.
- static constexpr double kMaxWristAngleForMovingByIntake = 0.20;
+ static constexpr double kMaxWristAngleForMovingByIntake = 0.50;
// The shoulder angle (in radians) below which the intake can safely move
// into the collision zone. This is necessary when the robot wants to fold up