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());