Check that ball speed is positive
Signed-off-by: Stephan Pleines <pleines.stephan@gmail.com>
Change-Id: I032e427e6dc0395b0664eb69d2eebbd245699026
diff --git a/frc971/control_loops/aiming/aiming.cc b/frc971/control_loops/aiming/aiming.cc
index 4cf9255..572ed1c 100644
--- a/frc971/control_loops/aiming/aiming.cc
+++ b/frc971/control_loops/aiming/aiming.cc
@@ -54,6 +54,8 @@
} // namespace
TurretGoal AimerGoal(const ShotConfig &config, const RobotState &state) {
+ CHECK(config.ball_speed_over_ground > 0.0)
+ << ": Ball speed must be positive.";
TurretGoal result;
// This code manages compensating the goal turret heading for the robot's
// current velocity, to allow for shooting on-the-fly.
diff --git a/frc971/control_loops/aiming/aiming_test.cc b/frc971/control_loops/aiming/aiming_test.cc
index 4ec7eb9..a4f7bf2 100644
--- a/frc971/control_loops/aiming/aiming_test.cc
+++ b/frc971/control_loops/aiming/aiming_test.cc
@@ -158,4 +158,17 @@
EXPECT_FLOAT_EQ(0.0, goal.velocity);
}
+// Tests that we fail if the ballspeed is zero. Can't hit anything if the ball
+// isn't moving.
+TEST(AimerDeathTest, ZeroBallSpeed) {
+ const Pose target({0.0, 0.0, 0.0}, 0.0);
+ Pose robot_pose({1.0, 1.0, 1.0}, 0.0);
+ const constants::Range range{-2.36, 2.36, -2.16, 2.16};
+ const double kBallSpeed = 0.0;
+ EXPECT_DEATH(AimerGoal(ShotConfig{target, ShotMode::kShootOnTheFly, range,
+ kBallSpeed, 0.0, 0.0},
+ RobotState{robot_pose, {0.0, 0.0}, 0.0, 0.0}),
+ "Ball speed must be positive.");
+}
+
} // namespace frc971::control_loops::aiming::testing