Relax collision avoidance if not holding game piece
Change-Id: I361f639460b1fcb2c7c2223d3e4f42ae743b9ad0
diff --git a/y2019/control_loops/superstructure/collision_avoidance.cc b/y2019/control_loops/superstructure/collision_avoidance.cc
index c999cac..9f7cc9b 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance.cc
@@ -16,6 +16,8 @@
constexpr double CollisionAvoidance::kIntakeInAngle;
constexpr double CollisionAvoidance::kWristElevatorCollisionMinAngle;
constexpr double CollisionAvoidance::kWristElevatorCollisionMaxAngle;
+constexpr double
+ CollisionAvoidance::kWristElevatorCollisionMaxAngleWithoutObject;
constexpr double CollisionAvoidance::kEps;
constexpr double CollisionAvoidance::kEpsIntake;
constexpr double CollisionAvoidance::kEpsWrist;
@@ -32,10 +34,15 @@
const double wrist_position = status->wrist.position;
const double elevator_position = status->elevator.position;
const double intake_position = status->intake.position;
+ const bool has_piece = status->has_piece;
+
+ const double wrist_elevator_collision_max_angle =
+ has_piece ? kWristElevatorCollisionMaxAngle
+ : kWristElevatorCollisionMaxAngleWithoutObject;
// Elevator is down, so the wrist can't be close to vertical.
if (elevator_position < kElevatorClearHeight) {
- if (wrist_position < kWristElevatorCollisionMaxAngle &&
+ if (wrist_position < wrist_elevator_collision_max_angle &&
wrist_position > kWristElevatorCollisionMinAngle) {
return true;
}
@@ -69,6 +76,7 @@
const double wrist_position = status->wrist.position;
const double elevator_position = status->elevator.position;
const double intake_position = status->intake.position;
+ const bool has_piece = status->has_piece;
// Start with our constraints being wide open.
clear_max_wrist_goal();
@@ -76,6 +84,10 @@
clear_max_intake_goal();
clear_min_intake_goal();
+ const double wrist_elevator_collision_max_angle =
+ has_piece ? kWristElevatorCollisionMaxAngle
+ : kWristElevatorCollisionMaxAngleWithoutObject;
+
// If the elevator is low enough, we also can't transition the wrist.
if (elevator_position < kElevatorClearHeight) {
// Figure out which side the wrist is on and stay there.
@@ -84,7 +96,7 @@
3.0) {
update_max_wrist_goal(kWristElevatorCollisionMinAngle - kEpsWrist);
} else {
- update_min_wrist_goal(kWristElevatorCollisionMaxAngle + kEpsWrist);
+ update_min_wrist_goal(wrist_elevator_collision_max_angle + kEpsWrist);
}
}
@@ -101,7 +113,7 @@
// If the elevator is too low, the intake can't transition from in to out or
// back.
if (elevator_position < kElevatorClearIntakeHeight &&
- wrist_position > kWristElevatorCollisionMaxAngle) {
+ wrist_position > wrist_elevator_collision_max_angle) {
// Figure out if the intake is in our out and keep it there.
if (intake_position < kIntakeMiddleAngle) {
update_max_intake_goal(kIntakeInAngle - kEpsIntake);
@@ -115,7 +127,7 @@
// If the intake is within the collision range, don't let the elevator down.
if (intake_position > kIntakeInAngle && intake_position < kIntakeOutAngle &&
- wrist_position > kWristElevatorCollisionMaxAngle) {
+ wrist_position > wrist_elevator_collision_max_angle) {
update_min_elevator_goal(kElevatorClearIntakeHeight + kEps);
}
@@ -129,7 +141,7 @@
// If the wrist is within the elevator collision range, don't let the elevator
// go down.
if (wrist_position > kWristElevatorCollisionMinAngle &&
- wrist_position < kWristElevatorCollisionMaxAngle) {
+ wrist_position < wrist_elevator_collision_max_angle) {
update_min_elevator_goal(kElevatorClearHeight + kEps);
}
@@ -154,7 +166,7 @@
// If we need to move the intake, we've got to shove the elevator up. The
// intake is already constrained so it can't hit anything until it's clear.
if (intake_needs_to_move &&
- wrist_position > kWristElevatorCollisionMaxAngle) {
+ wrist_position > wrist_elevator_collision_max_angle) {
update_min_elevator_goal(kElevatorClearIntakeHeight + kEps);
}
// If we need to move the wrist, we've got to shove the elevator up too. The