commit 42a87457e4a3845f9c836f30bccd449fccb43c25
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Fri Apr 05 17:30:47 2024 -0700
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Fri Apr 05 17:50:50 2024 -0700
tree eec7f7d7ab704f33c717cc66d46b9f3f7b5faddb
parent 592055e776b0bd3b2983a17bda1877d68e18a055

Make new climber fully work

Change-Id: Id7884fdf22a180a4a33d174638c188dc1e01ae48
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>

y2024/joystick_reader.cc

diff --git a/y2024/joystick_reader.cc b/y2024/joystick_reader.cc
index 5b2fc74..fcece24 100644
--- a/y2024/joystick_reader.cc
+++ b/y2024/joystick_reader.cc
@@ -94,7 +94,7 @@
         superstructure_goal_builder =
             superstructure_goal_sender_.MakeStaticBuilder();
 
-    if (data.IsPressed(kIntake)) {
+    if (data.IsPressed(kIntake) || climbed_count_ > 25) {
       // Intake is pressed
       superstructure_goal_builder->set_intake_pivot(
           superstructure::IntakePivotGoal::DOWN);
@@ -163,9 +163,16 @@
                                           data.IsPressed(kDriverFire));
 
     if (data.IsPressed(kRaiseClimber)) {
+      ++climbed_count_;
       superstructure_goal_builder->set_climber_goal_voltage(4.0);
     } else if (data.IsPressed(kRaiseFastClimber)) {
-      superstructure_goal_builder->set_climber_goal_voltage(6.0);
+      ++climbed_count_;
+      superstructure_goal_builder->set_climber_goal_voltage(12.0);
+    }
+
+    if (climbed_count_ > 25) {
+      PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
+          shooter_goal->add_turret_position(), 0.0);
     }
 
     superstructure_goal_builder.CheckOk(superstructure_goal_builder.Send());
@@ -177,6 +184,8 @@
   ::aos::Fetcher<control_loops::superstructure::Status>
       superstructure_status_fetcher_;
   const y2024::Constants *robot_constants_;
+
+  int climbed_count_ = 0;
 };
 
 }  // namespace y2024::input::joysticks