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
diff --git a/y2019/control_loops/superstructure/collision_avoidance.h b/y2019/control_loops/superstructure/collision_avoidance.h
index ca29e7c..7fc80cd 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.h
+++ b/y2019/control_loops/superstructure/collision_avoidance.h
@@ -70,6 +70,7 @@
// elevator is below kElevatorClearHeight.
static constexpr double kWristElevatorCollisionMinAngle = -M_PI / 4.0;
static constexpr double kWristElevatorCollisionMaxAngle = M_PI / 4.0;
+ static constexpr double kWristElevatorCollisionMaxAngleWithoutObject = M_PI / 6.0;
// Tolerance for the elevator.
static constexpr double kEps = 0.02;
diff --git a/y2019/control_loops/superstructure/collision_avoidance_tests.cc b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
index 918bc5e..82bff4c 100644
--- a/y2019/control_loops/superstructure/collision_avoidance_tests.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
@@ -22,8 +22,11 @@
QuarterCounterClockwiseRotationFromBottomFrontIntakeMoving
*/
-class CollisionAvoidanceTests : public ::testing::Test {
+class CollisionAvoidanceTests : public ::testing::Test,
+ public ::testing::WithParamInterface<bool> {
public:
+ CollisionAvoidanceTests() { status.has_piece = GetParam(); }
+
void Iterate() {
SuperstructureQueue::Goal safe_goal;
bool was_collided = avoidance.IsCollided(&status);
@@ -40,8 +43,8 @@
::aos::Clip(unsafe_goal.wrist.unsafe_goal, avoidance.min_wrist_goal(),
avoidance.max_wrist_goal());
- safe_goal.elevator.unsafe_goal = ::std::max(unsafe_goal.elevator.unsafe_goal,
- avoidance.min_elevator_goal());
+ safe_goal.elevator.unsafe_goal = ::std::max(
+ unsafe_goal.elevator.unsafe_goal, avoidance.min_elevator_goal());
safe_goal.intake.unsafe_goal =
::aos::Clip(unsafe_goal.intake.unsafe_goal,
@@ -79,7 +82,8 @@
void CheckGoals() {
// check to see if we reached the goals
ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
- ASSERT_NEAR(unsafe_goal.elevator.unsafe_goal, status.elevator.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.elevator.unsafe_goal, status.elevator.position,
+ 0.001);
ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
}
@@ -97,11 +101,12 @@
static constexpr double kIterationMove = 0.001;
};
// It is trying to rotate from far back to front low.
-TEST_F(CollisionAvoidanceTests, FullClockwiseRotationFromBottomBackIntakeIn) {
+TEST_P(CollisionAvoidanceTests, FullClockwiseRotationFromBottomBackIntakeIn) {
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// in.
- unsafe_goal.wrist.unsafe_goal = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ unsafe_goal.wrist.unsafe_goal =
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist;
unsafe_goal.elevator.unsafe_goal = 0.0;
unsafe_goal.intake.unsafe_goal =
avoidance.kIntakeInAngle - avoidance.kEpsIntake;
@@ -118,12 +123,13 @@
}
// It is trying to rotate from the front middle to front bottom.
-TEST_F(CollisionAvoidanceTests,
+TEST_P(CollisionAvoidanceTests,
QuarterClockwiseRotationFromMiddleFrontIntakeOut) {
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.unsafe_goal = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ unsafe_goal.wrist.unsafe_goal =
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist;
unsafe_goal.elevator.unsafe_goal = 0.0;
unsafe_goal.intake.unsafe_goal =
avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
@@ -140,7 +146,7 @@
CheckGoals();
}
// It is trying to rotate from the front middle to front bottom.
-TEST_F(CollisionAvoidanceTests,
+TEST_P(CollisionAvoidanceTests,
QuarterClockwiseRotationFromMiddleFrontIntakeMiddle) {
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
@@ -152,7 +158,8 @@
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- unsafe_goal.wrist.unsafe_goal = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ unsafe_goal.wrist.unsafe_goal =
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist;
unsafe_goal.elevator.unsafe_goal = 0.0;
unsafe_goal.intake.unsafe_goal =
avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
@@ -163,7 +170,7 @@
}
// It is trying to rotate from front low to far back.
-TEST_F(CollisionAvoidanceTests,
+TEST_P(CollisionAvoidanceTests,
FullCounterClockwiseRotationFromBottomBackIntakeIn) {
// sets the status position messgaes to be have the elevator at the bottom
// with the intake in and the wrist low back
@@ -174,7 +181,8 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// in.
- unsafe_goal.wrist.unsafe_goal = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+ unsafe_goal.wrist.unsafe_goal =
+ avoidance.kWristMinAngle + avoidance.kEpsWrist;
unsafe_goal.elevator.unsafe_goal = 0.0;
unsafe_goal.intake.unsafe_goal =
avoidance.kIntakeInAngle - avoidance.kEpsIntake;
@@ -185,7 +193,7 @@
}
// It is trying to rotate from the front bottom to front middle.
-TEST_F(CollisionAvoidanceTests,
+TEST_P(CollisionAvoidanceTests,
QuarterCounterClockwiseRotationFromMiddleFrontIntakeOut) {
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
@@ -208,7 +216,7 @@
}
// It is trying to rotate from the front bottom to front middle.
-TEST_F(CollisionAvoidanceTests,
+TEST_P(CollisionAvoidanceTests,
QuarterCounterClockwiseRotationFromBottomFrontIntakeMiddle) {
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
@@ -232,7 +240,7 @@
}
// Unreasonable Elevator Goal
-TEST_F(CollisionAvoidanceTests, UnreasonableElevatorGoal) {
+TEST_P(CollisionAvoidanceTests, UnreasonableElevatorGoal) {
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
@@ -256,7 +264,7 @@
}
// Unreasonable Wrist Goal
-TEST_F(CollisionAvoidanceTests, UnreasonableWristGoal) {
+TEST_P(CollisionAvoidanceTests, UnreasonableWristGoal) {
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
@@ -282,7 +290,7 @@
}
// Fix Collision Wrist in Elevator
-TEST_F(CollisionAvoidanceTests, FixElevatorCollision) {
+TEST_P(CollisionAvoidanceTests, FixElevatorCollision) {
// changes the goals
unsafe_goal.wrist.unsafe_goal = 3.14;
unsafe_goal.elevator.unsafe_goal = 0.0;
@@ -303,9 +311,10 @@
}
// Fix Collision Wrist in Intake
-TEST_F(CollisionAvoidanceTests, FixWristCollision) {
+TEST_P(CollisionAvoidanceTests, FixWristCollision) {
// changes the goals
- unsafe_goal.wrist.unsafe_goal = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ unsafe_goal.wrist.unsafe_goal =
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist;
unsafe_goal.elevator.unsafe_goal = 0.0;
unsafe_goal.intake.unsafe_goal =
(avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
@@ -324,7 +333,7 @@
ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
}
// Fix Collision Wrist Above Elevator
-TEST_F(CollisionAvoidanceTests, FixWristElevatorCollision) {
+TEST_P(CollisionAvoidanceTests, FixWristElevatorCollision) {
// changes the goals
unsafe_goal.wrist.unsafe_goal = 0.0;
unsafe_goal.elevator.unsafe_goal = 0.0;
@@ -343,6 +352,10 @@
status.elevator.position, 0.001);
ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
}
+
+INSTANTIATE_TEST_CASE_P(CollisionAvoidancePieceTest, CollisionAvoidanceTests,
+ ::testing::Bool());
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops