commit 46712f59b0ab9e59b21b4e8d5b334bc97ec95b35
author Austin Schuh <austin.linux@gmail.com> Sun Oct 24 22:24:45 2021 -0700
committer Austin Schuh <austin.linux@gmail.com> Wed Oct 27 20:09:36 2021 -0700
tree ba004fde7018ed3753fc005dd1e90de0e937d9a3
parent d0d894e64006bfd6745d3d4181518e0c38f463a6

Fix turret done

It was waiting for us to be near the profiled goal, not the final goal.
Whops.

Change-Id: I0aac4b08d18386d559aeb19f6d19d08676de6203
Signed-off-by: Austin Schuh <austin.linux@gmail.com>

y2020/control_loops/superstructure/superstructure.cc

diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 4c5f235..f62109c 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -234,14 +234,19 @@
     output_struct.washing_machine_spinner_voltage = 0.0;
     output_struct.feeder_voltage = 0.0;
     output_struct.intake_roller_voltage = 0.0;
+    output_struct.climber_voltage = 0.0;
     if (unsafe_goal) {
-      output_struct.climber_voltage =
-          std::clamp(unsafe_goal->climber_voltage(), -12.0f, 12.0f);
+      if (unsafe_goal->has_turret()) {
+        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;
+        // Make sure the turret is relatively close to the goal before turning
+        // the climber on.
+        CHECK(unsafe_goal->has_turret());
+        if (std::abs(unsafe_goal->turret()->unsafe_goal() -
+                     turret_.position()) > 0.1) {
+          output_struct.climber_voltage = 0;
+        }
       }
 
       if (unsafe_goal->shooting() || unsafe_goal->intake_preloading()) {