Check if the turret is ready before climbing
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I6765fdae22b401f1d4364e075af5e0bbf6706c07
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 86dcbb1..4c5f235 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -238,6 +238,12 @@
output_struct.climber_voltage =
std::clamp(unsafe_goal->climber_voltage(), -12.0f, 12.0f);
+ // Make sure the turret is relatively close to the goal before turning the
+ // climber on.
+ if (std::abs(turret_.goal(0) - turret_.position()) > 0.1) {
+ output_struct.climber_voltage = 0;
+ }
+
if (unsafe_goal->shooting() || unsafe_goal->intake_preloading()) {
preloading_timeout_ = position_timestamp + kPreloadingTimeout;
}