Tweak collision avoidance
We want to prioritize moving the wrist where the big suction cup is
clear as opposed to randomly when near 0.
Also, we don't need to lift the elevator if the wrist is pretty far up
to pull the intake in.
Change-Id: Iba34152a69e563894f96439908a2fea71aab6d8f
diff --git a/y2019/control_loops/superstructure/collision_avoidance.h b/y2019/control_loops/superstructure/collision_avoidance.h
index 5a8a772..ca29e7c 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.h
+++ b/y2019/control_loops/superstructure/collision_avoidance.h
@@ -50,8 +50,6 @@
min_elevator_goal_ = ::std::max(min_elevator_goal, min_elevator_goal_);
}
- // TODO(sabina): set all the numbers to correctly match the robot.
-
// Height above which we can move the wrist freely.
static constexpr double kElevatorClearHeight = 0.35;
@@ -61,8 +59,8 @@
static constexpr double kElevatorClearIntakeHeight = 0.4;
// Angle constraints for the wrist when below kElevatorClearDownHeight
- static constexpr double kWristMaxAngle = M_PI / 2.0;
- static constexpr double kWristMinAngle = -M_PI / 2.0;
+ static constexpr double kWristMaxAngle = M_PI / 2.0 + 0.05;
+ static constexpr double kWristMinAngle = -M_PI / 2.0 - 0.05;
// Angles outside of which the intake is fully clear of the wrist.
static constexpr double kIntakeOutAngle = 0.3;