commit 346cdb7741db55d482b4d5926bc69be59fde9b41
author Austin Schuh <austin.linux@gmail.com> Sun Nov 14 21:03:07 2021 -0800
committer Austin Schuh <austin.linux@gmail.com> Sat Nov 20 15:46:58 2021 -0800
tree 4df440ae0012b431ddfd7a98384b0fa4e691dcd9
parent 3777953b3f1ec39d3a481143166b5b9f2dfb4e0c

Toggle washing machine direction when we shake the intake

Travis says this avoids jamming better

Change-Id: I3f30e8a4a2ea8d26b9347f29b4929403099b5938
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 dce958f..2f07dc5 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -105,6 +105,8 @@
           output != nullptr ? &(output_struct.hood_voltage) : nullptr,
           status->fbb());
 
+  bool intake_out_jostle = false;
+
   if (unsafe_goal != nullptr) {
     if (unsafe_goal->shooting() &&
         shooting_start_time_ == aos::monotonic_clock::min_time) {
@@ -118,7 +120,9 @@
       if ((position_timestamp - shooting_start_time_) % (kPeriod * 2) <
           kPeriod) {
         intake_joint_.set_min_position(-0.25);
+        intake_out_jostle = false;
       } else {
+        intake_out_jostle = true;
         intake_joint_.set_min_position(-0.75);
       }
     } else {
@@ -284,7 +288,12 @@
             turret_ready) {
           output_struct.feeder_voltage = 12.0;
         }
-        output_struct.washing_machine_spinner_voltage = 5.0;
+
+        if (!intake_out_jostle) {
+          output_struct.washing_machine_spinner_voltage = 5.0;
+        } else {
+          output_struct.washing_machine_spinner_voltage = -5.0;
+        }
         output_struct.intake_roller_voltage = 3.0;
       } else {
         output_struct.intake_roller_voltage =