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

y2020/control_loops/superstructure/superstructure.h

diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index 59e976e..ebb8841 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -1,8 +1,8 @@
 #ifndef y2020_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
 #define y2020_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
 
-#include "frc971/control_loops/control_loop.h"
 #include "aos/events/event_loop.h"
+#include "frc971/control_loops/control_loop.h"
 #include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #include "frc971/input/joystick_state_generated.h"
 #include "y2020/constants.h"
@@ -29,6 +29,10 @@
   static constexpr double kTurretFrictionGain = 0.0;
   static constexpr double kTurretFrictionVoltageLimit = 1.5;
   static constexpr double kTurretDitherGain = 0.0;
+  static constexpr std::chrono::milliseconds kPreloadingTimeout =
+      std::chrono::seconds(5);
+  static constexpr std::chrono::milliseconds kPreloadingBackpowerDuration =
+      std::chrono::milliseconds(50);
 
   using PotAndAbsoluteEncoderSubsystem =
       ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
@@ -70,6 +74,10 @@
 
   aos::monotonic_clock::time_point shooting_start_time_ =
       aos::monotonic_clock::min_time;
+  aos::monotonic_clock::time_point preloading_timeout_ =
+      aos::monotonic_clock::min_time;
+  aos::monotonic_clock::time_point preloading_backpower_timeout_ =
+      aos::monotonic_clock::min_time;
 
   DISALLOW_COPY_AND_ASSIGN(Superstructure);
 };