commit ae16c3bf020e4a3fa51ec5d557656e9b242563ba
author Austin Schuh <austin.linux@gmail.com> Sat Oct 16 18:34:16 2021 -0700
committer Austin Schuh <austin.linux@gmail.com> Sat Oct 16 20:56:02 2021 -0700
tree 193ec0a39382fce221bb3ed597cf50fa3a6aefb6
parent 30e45ffdb4e35e68a58d56c3bbd5c16aa3cf80d1

Make sure turret is ready before taking the shot

Change-Id: I0d06a9389234d35a63bc53954df64cf8de345f3e
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 b2390ce..72fb1ac 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -233,7 +233,8 @@
 
       if (unsafe_goal->shooting()) {
         if (shooter_.ready() && shooter_.finisher_goal() > 10.0 &&
-            shooter_.accelerator_goal() > 10.0) {
+            shooter_.accelerator_goal() > 10.0 &&
+            std::abs(turret_.goal(0) - turret_.position()) < 0.025) {
           output_struct.feeder_voltage = 12.0;
         }
         output_struct.washing_machine_spinner_voltage = 5.0;