Add catapult to collision avoidance
Signed-off-by: Henry Speiser <henry@speiser.net>
Change-Id: I504fbfc23d549f25c8e345818f7ee84f183508c6
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
diff --git a/y2022/control_loops/superstructure/collision_avoidance.cc b/y2022/control_loops/superstructure/collision_avoidance.cc
index d87054a..0c13295 100644
--- a/y2022/control_loops/superstructure/collision_avoidance.cc
+++ b/y2022/control_loops/superstructure/collision_avoidance.cc
@@ -33,6 +33,25 @@
return true;
}
+ // If we aren't firing, no need to check the catapult
+ if (!status.shooting) {
+ return false;
+ }
+
+ // Checks if intake front is collided with catapult.
+ if (TurretCollided(
+ status.intake_front_position, status.turret_position + M_PI,
+ kMinCollisionZoneFrontTurret, kMaxCollisionZoneFrontTurret)) {
+ return true;
+ }
+
+ // Checks if intake back is collided with catapult.
+ if (TurretCollided(status.intake_back_position, status.turret_position + M_PI,
+ kMinCollisionZoneBackTurret,
+ kMaxCollisionZoneBackTurret)) {
+ return true;
+ }
+
return false;
}
@@ -96,20 +115,39 @@
// Calculating the avoidance with either intake, and when the turret is
// wrapped.
- CalculateAvoidance(true, intake_front_position, turret_goal,
- kMinCollisionZoneFrontTurret, kMaxCollisionZoneFrontTurret,
- turret_position);
- CalculateAvoidance(false, intake_back_position, turret_goal,
- kMinCollisionZoneBackTurret, kMaxCollisionZoneBackTurret,
- turret_position);
+ CalculateAvoidance(true, false, intake_front_position, turret_position,
+ turret_goal, kMinCollisionZoneFrontTurret,
+ kMaxCollisionZoneFrontTurret);
+ CalculateAvoidance(false, false, intake_back_position, turret_position,
+ turret_goal, kMinCollisionZoneBackTurret,
+ kMaxCollisionZoneBackTurret);
+
+ // If we aren't firing, no need to check the catapult
+ if (!status.shooting) {
+ return;
+ }
+
+ CalculateAvoidance(true, true, intake_front_position, turret_position,
+ turret_goal, kMinCollisionZoneFrontTurret,
+ kMaxCollisionZoneFrontTurret);
+ CalculateAvoidance(false, true, intake_back_position, turret_position,
+ turret_goal, kMinCollisionZoneBackTurret,
+ kMaxCollisionZoneBackTurret);
}
-void CollisionAvoidance::CalculateAvoidance(bool intake_front,
+void CollisionAvoidance::CalculateAvoidance(bool intake_front, bool catapult,
double intake_position,
+ double turret_position,
double turret_goal,
double min_turret_collision_goal,
- double max_turret_collision_goal,
- double turret_position) {
+ double max_turret_collision_goal) {
+ // If we are checking the catapult, offset the turret angle to represent where
+ // the catapult is
+ if (catapult) {
+ turret_position += M_PI;
+ turret_goal += M_PI;
+ }
+
auto [turret_position_wrapped, turret_position_wraps] =
WrapTurretAngle(turret_position);
@@ -161,12 +199,15 @@
// If the intake is in the way, limit the turret until moved. Otherwise,
// let'errip!
if (!turret_pos_unsafe && (intake_position > kCollisionZoneIntake)) {
+ // If we were comparing the position of the catapult,
+ // remove that offset of pi to get the turret position
+ const double bounds_offset = (catapult ? -M_PI : 0);
if (turret_position < min_turret_collision_goal_unwrapped) {
- update_max_turret_goal(min_turret_collision_goal_unwrapped -
- kEpsTurret);
+ update_max_turret_goal(min_turret_collision_goal_unwrapped +
+ bounds_offset - kEpsTurret);
} else {
update_min_turret_goal(max_turret_collision_goal_unwrapped +
- kEpsTurret);
+ bounds_offset + kEpsTurret);
}
}
}
diff --git a/y2022/control_loops/superstructure/collision_avoidance.h b/y2022/control_loops/superstructure/collision_avoidance.h
index b407409..3cd5344 100644
--- a/y2022/control_loops/superstructure/collision_avoidance.h
+++ b/y2022/control_loops/superstructure/collision_avoidance.h
@@ -24,6 +24,8 @@
bool AngleInRange(double theta, double theta_min, double theta_max);
// 1. Prevent the turret from moving if the intake is up
+// and prevent the back of the turret (where the catapult is)
+// from colliding with the intake when it's up.
// 2. If the intake is up, drop it so it is not in the way
// 3. Move the turret to the desired position.
// 4. When the turret moves away, if the intake is down, move it back up.
@@ -33,11 +35,12 @@
double intake_front_position;
double intake_back_position;
double turret_position;
+ bool shooting;
bool operator==(const Status &s) const {
return (intake_front_position == s.intake_front_position &&
intake_back_position == s.intake_back_position &&
- turret_position == s.turret_position);
+ turret_position == s.turret_position && shooting == s.shooting);
}
bool operator!=(const Status &s) const { return !(*this == s); }
};
@@ -60,9 +63,8 @@
// Maximum position of the intake to avoid collisions
static constexpr double kCollisionZoneIntake = 1.4;
- // Tolerance for the turret.
+ // Tolerances for the subsystems
static constexpr double kEpsTurret = 0.05;
- // Tolerance for the intake.
static constexpr double kEpsIntake = 0.05;
CollisionAvoidance();
@@ -79,10 +81,10 @@
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
*unsafe_turret_goal);
// Limits if goal is in collision spots.
- void CalculateAvoidance(bool intake_front, double intake_position,
- double turret_goal, double mix_turret_collision_goal,
- double max_turret_collision_goal,
- double turret_position);
+ void CalculateAvoidance(bool intake_front, bool catapult,
+ double intake_position, double turret_position,
+ double turret_goal, double min_turret_collision_goal,
+ double max_turret_collision_goal);
// Returns the goals to give to the respective control loops in
// superstructure.
diff --git a/y2022/control_loops/superstructure/collision_avoidance_test.cc b/y2022/control_loops/superstructure/collision_avoidance_test.cc
index 44dee44..cd54fea 100644
--- a/y2022/control_loops/superstructure/collision_avoidance_test.cc
+++ b/y2022/control_loops/superstructure/collision_avoidance_test.cc
@@ -76,13 +76,14 @@
kNegativeUnsafeFrontWrapped,
kNegativeUnsafeBackWrapped,
};
+enum class CatapultState { kIdle, kShooting };
class CollisionAvoidanceTest : public ::testing::Test {
public:
CollisionAvoidanceTest()
: unsafe_goal_(MakeZeroGoal()),
- status_({0.0, 0.0, 0.0}),
- prev_status_({0.0, 0.0, 0.0}) {}
+ status_({0.0, 0.0, 0.0, false}),
+ prev_status_({0.0, 0.0, 0.0, false}) {}
void Simulate() {
FlatbufferDetachedBuffer<Goal> safe_goal = MakeZeroGoal();
@@ -232,10 +233,12 @@
void Test(IntakeState intake_front_pos_state,
IntakeState intake_back_pos_state, TurretState turret_pos_state,
IntakeState intake_front_goal_state,
- IntakeState intake_back_goal_state, TurretState turret_goal_state) {
+ IntakeState intake_back_goal_state, TurretState turret_goal_state,
+ CatapultState catapult_state) {
status_ = {ComputeIntakeAngle(intake_front_pos_state),
ComputeIntakeAngle(intake_back_pos_state),
- ComputeTurretAngle(turret_pos_state)};
+ ComputeTurretAngle(turret_pos_state),
+ catapult_state == CatapultState::kShooting};
unsafe_goal_.mutable_message()->mutable_intake_front()->mutate_unsafe_goal(
ComputeIntakeAngle(intake_front_goal_state));
@@ -260,13 +263,18 @@
// Turret is highest priority and should always reach the unsafe goal
EXPECT_NEAR(turret_goal(), status_.turret_position, kIterationMove);
- // If the unsafe goal had an intake colliding with the turret, the intake
- // position should be at least the collision zone angle.
+ // If the unsafe goal had an intake colliding with the turret or catapult,
+ // the intake position should be at least the collision zone angle.
// Otherwise, the intake should be at the unsafe goal
if (avoidance_.TurretCollided(
intake_front_goal(), turret_goal(),
CollisionAvoidance::kMinCollisionZoneFrontTurret,
- CollisionAvoidance::kMaxCollisionZoneFrontTurret)) {
+ CollisionAvoidance::kMaxCollisionZoneFrontTurret) ||
+ (status_.shooting &&
+ avoidance_.TurretCollided(
+ intake_front_goal(), turret_goal() + M_PI,
+ CollisionAvoidance::kMinCollisionZoneFrontTurret,
+ CollisionAvoidance::kMaxCollisionZoneFrontTurret))) {
EXPECT_LE(status_.intake_front_position,
CollisionAvoidance::kCollisionZoneIntake);
} else {
@@ -277,7 +285,12 @@
if (avoidance_.TurretCollided(
intake_back_goal(), turret_goal(),
CollisionAvoidance::kMinCollisionZoneBackTurret,
- CollisionAvoidance::kMaxCollisionZoneBackTurret)) {
+ CollisionAvoidance::kMaxCollisionZoneBackTurret) ||
+ (status_.shooting &&
+ avoidance_.TurretCollided(
+ intake_back_goal(), turret_goal() + M_PI,
+ CollisionAvoidance::kMinCollisionZoneBackTurret,
+ CollisionAvoidance::kMaxCollisionZoneBackTurret))) {
EXPECT_LE(status_.intake_back_position,
CollisionAvoidance::kCollisionZoneIntake);
} else {
@@ -338,8 +351,13 @@
TurretState::kUnsafeFront, TurretState::kUnsafeBack,
TurretState::kUnsafeFrontWrapped,
TurretState::kUnsafeBackWrapped}) {
- Test(intake_front_pos, intake_back_pos, turret_pos,
- intake_front_goal, intake_back_goal, turret_goal);
+ // Catapult state
+ for (CatapultState catapult_state :
+ {CatapultState::kIdle, CatapultState::kShooting}) {
+ Test(intake_front_pos, intake_back_pos, turret_pos,
+ intake_front_goal, intake_back_goal, turret_goal,
+ catapult_state);
+ }
}
}
}
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index b9e0698..bb8d4d0 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -279,9 +279,15 @@
turret_goal != nullptr &&
std::abs(turret_goal->unsafe_goal() - turret_.position()) <
kTurretGoalThreshold;
+ const bool collided = collision_avoidance_.IsCollided(
+ {.intake_front_position = intake_front_.estimated_position(),
+ .intake_back_position = intake_back_.estimated_position(),
+ .turret_position = turret_.estimated_position(),
+ .shooting = true});
- // If the turret reached the aiming goal, fire!
- if (flippers_open_ && turret_near_goal) {
+ // If the turret reached the aiming goal and the catapult is safe to move
+ // up, fire!
+ if (flippers_open_ && turret_near_goal && !collided) {
fire_ = true;
}
@@ -312,7 +318,8 @@
collision_avoidance_.UpdateGoal(
{.intake_front_position = intake_front_.estimated_position(),
.intake_back_position = intake_back_.estimated_position(),
- .turret_position = turret_.estimated_position()},
+ .turret_position = turret_.estimated_position(),
+ .shooting = state_ == SuperstructureState::SHOOTING},
turret_goal);
turret_.set_min_position(collision_avoidance_.min_turret_goal());
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index cd5929b..351601e 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -1056,9 +1056,9 @@
TEST_F(SuperstructureTest, ShootCatapult) {
SetEnabled(true);
superstructure_plant_.intake_front()->InitializePosition(
- constants::Values::kIntakeRange().upper);
+ constants::Values::kIntakeRange().lower);
superstructure_plant_.intake_back()->InitializePosition(
- constants::Values::kIntakeRange().upper);
+ constants::Values::kIntakeRange().lower);
superstructure_plant_.turret()->InitializePosition(
constants::Values::kTurretFrontIntakePos());
@@ -1179,8 +1179,7 @@
goal_builder.add_auto_aim(true);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
// Give it time to stabilize.