Make Aimer use correct turret zero convention
Turns out zero is pointed backwards.
Change-Id: I1132152d9ff4edea9464468c2aa746b40bcffc8b
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 641f430..9a86788 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -880,9 +880,9 @@
RunFor(chrono::seconds(1));
superstructure_status_fetcher_.Fetch();
- EXPECT_FLOAT_EQ(kShotAngle - M_PI,
+ EXPECT_FLOAT_EQ(kShotAngle,
superstructure_status_fetcher_->turret()->position());
- EXPECT_FLOAT_EQ(kShotAngle - M_PI,
+ EXPECT_FLOAT_EQ(kShotAngle,
superstructure_status_fetcher_->aimer()->turret_position());
EXPECT_FLOAT_EQ(0,
superstructure_status_fetcher_->aimer()->turret_velocity());
diff --git a/y2020/control_loops/superstructure/turret/aiming.cc b/y2020/control_loops/superstructure/turret/aiming.cc
index 7aaefac..360fd1a 100644
--- a/y2020/control_loops/superstructure/turret/aiming.cc
+++ b/y2020/control_loops/superstructure/turret/aiming.cc
@@ -77,6 +77,10 @@
// kAntiWrapBuffer radians away from the hardstops.
constexpr double kAntiWrapBuffer = 0.2;
+// If the turret is at zero, then it will be at this angle relative to pointed
+// straight forwards on the robot.
+constexpr double kTurretZeroOffset = M_PI;
+
constexpr double kTurretRange = constants::Values::kTurretRange().range();
static_assert((kTurretRange - 2.0 * kAntiWrapBuffer) > 2.0 * M_PI,
"kAntiWrap buffer should be small enough that we still have 360 "
@@ -220,9 +224,10 @@
// would have to travel).
// We then check if this goal would bring us out of range of the valid angles,
// and if it would, we reset to be within +/- pi of zero.
- double turret_heading = goal_.message().unsafe_goal() +
- aos::math::NormalizeAngle(
- heading_to_goal - goal_.message().unsafe_goal());
+ double turret_heading =
+ goal_.message().unsafe_goal() +
+ aos::math::NormalizeAngle(heading_to_goal - kTurretZeroOffset -
+ goal_.message().unsafe_goal());
if (std::abs(turret_heading - constants::Values::kTurretRange().middle()) >
range / 2.0) {
turret_heading = aos::math::NormalizeAngle(turret_heading);
diff --git a/y2020/control_loops/superstructure/turret/aiming_test.cc b/y2020/control_loops/superstructure/turret/aiming_test.cc
index ef81252..cb95b56 100644
--- a/y2020/control_loops/superstructure/turret/aiming_test.cc
+++ b/y2020/control_loops/superstructure/turret/aiming_test.cc
@@ -66,7 +66,7 @@
.theta = 0.0,
.linear = 0.0,
.angular = 0.0});
- EXPECT_EQ(M_PI, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->unsafe_goal());
EXPECT_EQ(0.0, goal->goal_velocity());
EXPECT_EQ(1.0, aimer_.DistanceToGoal());
goal = Update({.x = target.abs_pos().x() + 1.0,
@@ -74,14 +74,14 @@
.theta = 1.0,
.linear = 0.0,
.angular = 0.0});
- EXPECT_EQ(M_PI - 1.0, goal->unsafe_goal());
+ EXPECT_EQ(-1.0, goal->unsafe_goal());
EXPECT_EQ(0.0, goal->goal_velocity());
goal = Update({.x = target.abs_pos().x() + 1.0,
.y = target.abs_pos().y() + 0.0,
.theta = -1.0,
.linear = 0.0,
.angular = 0.0});
- EXPECT_EQ(-M_PI + 1.0, aos::math::NormalizeAngle(goal->unsafe_goal()));
+ EXPECT_EQ(1.0, aos::math::NormalizeAngle(goal->unsafe_goal()));
EXPECT_EQ(0.0, goal->goal_velocity());
EXPECT_EQ(1.0, aimer_.DistanceToGoal());
// Test that we handle the case that where we are right on top of the target.
@@ -90,7 +90,7 @@
.theta = 0.0,
.linear = 0.0,
.angular = 0.0});
- EXPECT_EQ(0.0, goal->unsafe_goal());
+ EXPECT_EQ(M_PI, goal->unsafe_goal());
EXPECT_EQ(0.0, goal->goal_velocity());
EXPECT_EQ(0.0, aimer_.DistanceToGoal());
}
@@ -102,7 +102,7 @@
.theta = 0.0,
.linear = 0.0,
.angular = 1.0});
- EXPECT_EQ(M_PI, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->unsafe_goal());
EXPECT_FLOAT_EQ(-1.0, goal->goal_velocity());
}
@@ -119,19 +119,19 @@
.angular = 0.0},
aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
Aimer::ShotMode::kStatic);
- EXPECT_EQ(M_PI, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->unsafe_goal());
EXPECT_FLOAT_EQ(0.0, goal->goal_velocity());
EXPECT_EQ(1.0, aimer_.DistanceToGoal());
// Next, try with shooting-on-the-fly enabled--because we are driving straight
// towards the target, only the goal distance should be impacted.
goal = Update({.x = target.abs_pos().x() + 1.0,
- .y = target.abs_pos().y() + 0.0,
- .theta = 0.0,
- .linear = 1.0,
- .angular = 0.0},
- aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
- Aimer::ShotMode::kShootOnTheFly);
- EXPECT_EQ(M_PI, goal->unsafe_goal());
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 1.0,
+ .angular = 0.0},
+ aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
+ Aimer::ShotMode::kShootOnTheFly);
+ EXPECT_EQ(0.0, goal->unsafe_goal());
EXPECT_FLOAT_EQ(0.0, goal->goal_velocity());
EXPECT_LT(1.0001, aimer_.DistanceToGoal());
EXPECT_GT(1.1, aimer_.DistanceToGoal());
@@ -149,7 +149,7 @@
.angular = 0.0},
aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
Aimer::ShotMode::kStatic);
- EXPECT_EQ(-M_PI_2, goal->unsafe_goal());
+ EXPECT_EQ(M_PI_2, goal->unsafe_goal());
EXPECT_FLOAT_EQ(-1.0, goal->goal_velocity());
EXPECT_EQ(1.0, aimer_.DistanceToGoal());
// Next, test with shooting-on-the-fly enabled, The goal numbers should all be
@@ -163,8 +163,8 @@
Aimer::ShotMode::kShootOnTheFly);
// Confirm that the turret heading goal is less then -pi / 2, but not by too
// much.
- EXPECT_GT(-M_PI_2 - 0.001, goal->unsafe_goal());
- EXPECT_LT(-M_PI_2 - 0.1, goal->unsafe_goal());
+ EXPECT_GT(M_PI_2 - 0.001, goal->unsafe_goal());
+ EXPECT_LT(M_PI_2 - 0.1, goal->unsafe_goal());
// Similarly, the turret velocity goal should be a bit greater than -1.0,
// since the turret is no longer at exactly a right angle.
EXPECT_LT(-1.0, goal->goal_velocity());
@@ -184,7 +184,7 @@
.linear = 0.0,
.angular = 0.0},
aos::Alliance::kRed);
- EXPECT_EQ(M_PI, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->unsafe_goal());
EXPECT_EQ(0.0, goal->goal_velocity());
}
@@ -199,17 +199,17 @@
.linear = 0.0,
.angular = 0.0};
const Goal *goal = Update(status);
- EXPECT_EQ(M_PI, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->unsafe_goal());
EXPECT_EQ(0.0, goal->goal_velocity());
// Move the robot a small amount--we should go past pi and not wrap yet.
status.theta = -0.1;
goal = Update(status);
- EXPECT_FLOAT_EQ(M_PI + 0.1, goal->unsafe_goal());
+ EXPECT_FLOAT_EQ(0.1, goal->unsafe_goal());
EXPECT_EQ(0.0, goal->goal_velocity());
// Move the robot so that, if we had no hard-stops, we would go past it.
status.theta = -2.0;
goal = Update(status);
- EXPECT_FLOAT_EQ(-M_PI + 2.0, goal->unsafe_goal());
+ EXPECT_FLOAT_EQ(2.0, goal->unsafe_goal());
EXPECT_EQ(0.0, goal->goal_velocity());
}
@@ -218,7 +218,7 @@
TEST_F(AimerTest, WrappingModes) {
// Start ourselves needing a turret angle of M_PI.
const Pose target = OuterPortPose(aos::Alliance::kBlue);
- StatusData status{.x = target.abs_pos().x() + 1.0,
+ StatusData status{.x = target.abs_pos().x() - 1.0,
.y = target.abs_pos().y() + 0.0,
.theta = 0.0,
.linear = 0.0,