Merge "Tweak collision avoidance"
diff --git a/y2019/control_loops/superstructure/collision_avoidance.cc b/y2019/control_loops/superstructure/collision_avoidance.cc
index ed6787f..c999cac 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance.cc
@@ -52,7 +52,8 @@
}
// Elevator is down so the intake has to be at either extreme.
- if (elevator_position < kElevatorClearIntakeHeight) {
+ if (elevator_position < kElevatorClearIntakeHeight &&
+ wrist_position > kWristMaxAngle) {
if (intake_position < kIntakeOutAngle && intake_position > kIntakeInAngle) {
return true;
}
@@ -78,7 +79,9 @@
// 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.
- if (wrist_position < 0.0) {
+ if (wrist_position < (2.0 * kWristElevatorCollisionMinAngle +
+ kWristElevatorCollisionMaxAngle) /
+ 3.0) {
update_max_wrist_goal(kWristElevatorCollisionMinAngle - kEpsWrist);
} else {
update_min_wrist_goal(kWristElevatorCollisionMaxAngle + kEpsWrist);
@@ -97,7 +100,8 @@
// If the elevator is too low, the intake can't transition from in to out or
// back.
- if (elevator_position < kElevatorClearIntakeHeight) {
+ if (elevator_position < kElevatorClearIntakeHeight &&
+ wrist_position > kWristElevatorCollisionMaxAngle) {
// Figure out if the intake is in our out and keep it there.
if (intake_position < kIntakeMiddleAngle) {
update_max_intake_goal(kIntakeInAngle - kEpsIntake);
@@ -110,10 +114,18 @@
clear_min_elevator_goal();
// If the intake is within the collision range, don't let the elevator down.
- if (intake_position > kIntakeInAngle && intake_position < kIntakeOutAngle) {
+ if (intake_position > kIntakeInAngle && intake_position < kIntakeOutAngle &&
+ wrist_position > kWristElevatorCollisionMaxAngle) {
update_min_elevator_goal(kElevatorClearIntakeHeight + kEps);
}
+ // If the intake is in the collision range and the elevator is down, don't let
+ // the wrist go far down.
+ if (intake_position > kIntakeInAngle && intake_position < kIntakeOutAngle &&
+ elevator_position < kElevatorClearIntakeHeight) {
+ update_max_wrist_goal(kWristMaxAngle - kEpsWrist);
+ }
+
// If the wrist is within the elevator collision range, don't let the elevator
// go down.
if (wrist_position > kWristElevatorCollisionMinAngle &&
@@ -141,7 +153,8 @@
// 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 > 0) {
+ if (intake_needs_to_move &&
+ wrist_position > kWristElevatorCollisionMaxAngle) {
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 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;
diff --git a/y2019/control_loops/superstructure/collision_avoidance_tests.cc b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
index 17f9fdc..918bc5e 100644
--- a/y2019/control_loops/superstructure/collision_avoidance_tests.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
@@ -260,24 +260,25 @@
// 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.kWristMinAngle;
+ 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;
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ status.wrist.position = avoidance.kWristMinAngle + avoidance.kEpsWrist;
status.elevator.position = 0.45;
status.intake.position =
(avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
- ASSERT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
+ EXPECT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
+ EXPECT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
+ EXPECT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
}
// Fix Collision Wrist in Elevator
@@ -304,13 +305,13 @@
// Fix Collision Wrist in Intake
TEST_F(CollisionAvoidanceTests, FixWristCollision) {
// changes the goals
- unsafe_goal.wrist.unsafe_goal = avoidance.kWristMinAngle + 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;
// sets the status position messgaes
- status.wrist.position = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+ status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
status.elevator.position = 0.0;
status.intake.position =
(avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;