Make sure velocity is close before shooting
Change-Id: I0d24b05083e71eaa6f27fa55a819a1241aa891c2
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index 7002657..b280b2f 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -167,10 +167,12 @@
const bool turret_in_range =
(std::abs(turret_.estimated_position() - turret_goal->unsafe_goal()) <
- kCatapultActivationTurretThreshold);
+ kCatapultActivationTurretThreshold) &&
+ (std::abs(turret_.estimated_velocity()) < 0.2);
const bool altitude_in_range =
(std::abs(altitude_.estimated_position() - altitude_goal->unsafe_goal()) <
- kCatapultActivationAltitudeThreshold);
+ kCatapultActivationAltitudeThreshold) &&
+ (std::abs(altitude_.estimated_velocity()) < 0.1);
const bool altitude_above_min_angle =
(altitude_.estimated_position() >
robot_constants_->common()->min_altitude_shooting_angle());
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index 9e8b3a5..a4e47d7 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -1272,7 +1272,7 @@
// Wait until the bot finishes auto-aiming.
WaitUntilNear(kTurretGoal, kAltitudeGoal);
- RunFor(dt());
+ RunFor(10 * dt());
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());