commit ac850dafef73581b718555ecdc0f3fd89cb0efe4
author Ravago Jones <ravagojones@gmail.com> Wed Oct 13 20:38:29 2021 -0700
committer Ravago Jones <ravagojones@gmail.com> Wed Oct 13 20:46:07 2021 -0700
tree 05b57c5ca4d8e92112dfa85a9c88e13552bac8ab
parent f852a5a40909a6780599746921d1c3ff57007a7c

Add timeout and backpower to ball preloading

A timeout to continue preserializing after a request
and backpowering to prevent accidental shots.

Change-Id: I7e6975bf2493562811fc14ecfcd6a91c0d0f2860
Signed-off-by: Ravago Jones <ravagojones@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 4d446ee..b2390ce 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -213,10 +213,22 @@
     output_struct.feeder_voltage = 0.0;
     output_struct.intake_roller_voltage = 0.0;
     if (unsafe_goal) {
-      if ((unsafe_goal->shooting() || unsafe_goal->intake_preloading()) &&
+      if (unsafe_goal->shooting() || unsafe_goal->intake_preloading()) {
+        preloading_timeout_ = position_timestamp + kPreloadingTimeout;
+      }
+
+      if (position_timestamp <= preloading_timeout_ &&
           !position->intake_beambreak_triggered()) {
         output_struct.washing_machine_spinner_voltage = 5.0;
         output_struct.feeder_voltage = 12.0;
+
+        preloading_backpower_timeout_ =
+            position_timestamp + kPreloadingBackpowerDuration;
+      }
+
+      if (position->intake_beambreak_triggered() &&
+          position_timestamp <= preloading_backpower_timeout_) {
+        output_struct.feeder_voltage = -12.0;
       }
 
       if (unsafe_goal->shooting()) {