Fix collision avoidance min turret angle
We wrap from 0 to 2 pi, but this was a negative angle...
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I97a00201fd68b572d4fbdb77440f5598f7a6f720
diff --git a/y2022/control_loops/superstructure/collision_avoidance.cc b/y2022/control_loops/superstructure/collision_avoidance.cc
index 01b0463..d87054a 100644
--- a/y2022/control_loops/superstructure/collision_avoidance.cc
+++ b/y2022/control_loops/superstructure/collision_avoidance.cc
@@ -47,6 +47,12 @@
return wrapped + 2.0 * M_PI * wraps;
}
+bool AngleInRange(double theta, double theta_min, double theta_max) {
+ return (
+ (theta >= theta_min && theta <= theta_max) ||
+ (theta_min > theta_max && (theta >= theta_min || theta <= theta_max)));
+}
+
bool CollisionAvoidance::TurretCollided(double intake_position,
double turret_position,
double min_turret_collision_position,
@@ -55,8 +61,8 @@
const double turret_position_wrapped = turret_position_wrapped_pair.first;
// Checks if turret is in the collision area.
- if (turret_position_wrapped >= min_turret_collision_position &&
- turret_position_wrapped <= max_turret_collision_position) {
+ if (AngleInRange(turret_position_wrapped, min_turret_collision_position,
+ max_turret_collision_position)) {
// Reterns true if the intake is raised.
if (intake_position > kCollisionZoneIntake) {
return true;
@@ -110,8 +116,8 @@
// If the turret goal is in a collison zone or moving through one, limit
// intake.
const bool turret_pos_unsafe =
- (turret_position_wrapped >= min_turret_collision_goal &&
- turret_position_wrapped <= max_turret_collision_goal);
+ AngleInRange(turret_position_wrapped, min_turret_collision_goal,
+ max_turret_collision_goal);
const bool turret_moving_forward = (turret_goal > turret_position);
@@ -129,8 +135,11 @@
}
min_turret_collision_goal_unwrapped =
UnwrapTurretAngle(min_turret_collision_goal, bounds_wraps);
+ // If we are checking the back intake, the max turret angle is on the wrap
+ // after the min, so add 1 to the number of wraps for it
const double max_turret_collision_goal_unwrapped =
- UnwrapTurretAngle(max_turret_collision_goal, bounds_wraps);
+ UnwrapTurretAngle(max_turret_collision_goal,
+ intake_front ? bounds_wraps : bounds_wraps + 1);
// Check if the closest unwrapped angles are going to be passed
const bool turret_moving_past_intake =
diff --git a/y2022/control_loops/superstructure/collision_avoidance.h b/y2022/control_loops/superstructure/collision_avoidance.h
index a64bbd3..b407409 100644
--- a/y2022/control_loops/superstructure/collision_avoidance.h
+++ b/y2022/control_loops/superstructure/collision_avoidance.h
@@ -19,6 +19,10 @@
// Returns the absolute angle given the wrapped angle and number of wraps.
double UnwrapTurretAngle(double wrapped, int wraps);
+// Checks if theta is between theta_min and theta_max. Expects all angles to be
+// wrapped from 0 to 2pi
+bool AngleInRange(double theta, double theta_min, double theta_max);
+
// 1. Prevent the turret from moving if the intake is up
// 2. If the intake is up, drop it so it is not in the way
// 3. Move the turret to the desired position.
@@ -49,7 +53,8 @@
static constexpr double kMaxCollisionZoneFrontTurret =
M_PI + kCollisionZoneTurret;
- static constexpr double kMinCollisionZoneBackTurret = -kCollisionZoneTurret;
+ static constexpr double kMinCollisionZoneBackTurret =
+ (2.0 * M_PI) - kCollisionZoneTurret;
static constexpr double kMaxCollisionZoneBackTurret = kCollisionZoneTurret;
// Maximum position of the intake to avoid collisions
diff --git a/y2022/control_loops/superstructure/collision_avoidance_test.cc b/y2022/control_loops/superstructure/collision_avoidance_test.cc
index adf672b..44dee44 100644
--- a/y2022/control_loops/superstructure/collision_avoidance_test.cc
+++ b/y2022/control_loops/superstructure/collision_avoidance_test.cc
@@ -416,4 +416,12 @@
}
}
+// Test that AngleInRange works correctly for wrapped angles
+TEST(AngleTest, AngleInRange) {
+ EXPECT_TRUE(AngleInRange(0.5, 0.4, 0.6));
+ EXPECT_TRUE(AngleInRange(0, (2.0 * M_PI) - 0.2, 0.2));
+ EXPECT_FALSE(AngleInRange(0, (2.0 * M_PI) - 0.2, (2.0 * M_PI) - 0.1));
+ EXPECT_TRUE(AngleInRange(0.5, (2.0 * M_PI) - 0.1, 0.6));
+}
+
} // namespace y2022::control_loops::superstructure::testing