From 4c8c4497767db9d6941e18560126d94c58c89486 Mon Sep 17 00:00:00 2001
From: Bradley Bickford <bradleybickford@gmail.com>
Date: Tue, 27 Feb 2024 17:33:14 -0500
Subject: [PATCH] before trailer load testing TO GRANITE STATE

---
 .../pathplanner/autos/Right Subwoofer 1S.auto       |  6 ------
 src/main/java/frc/robot/Commands/AmpHandoff.java    |  2 +-
 src/main/java/frc/robot/RobotContainer.java         | 13 ++++++++++---
 src/main/java/frc/robot/subsystems/Shooter.java     |  7 ++++---
 4 files changed, 15 insertions(+), 13 deletions(-)

diff --git a/src/main/deploy/pathplanner/autos/Right Subwoofer 1S.auto b/src/main/deploy/pathplanner/autos/Right Subwoofer 1S.auto
index b7048f2..166a05f 100644
--- a/src/main/deploy/pathplanner/autos/Right Subwoofer 1S.auto	
+++ b/src/main/deploy/pathplanner/autos/Right Subwoofer 1S.auto	
@@ -17,12 +17,6 @@
             "name": "Charge Shooter"
           }
         },
-        {
-          "type": "wait",
-          "data": {
-            "waitTime": 2.0
-          }
-        },
         {
           "type": "named",
           "data": {
diff --git a/src/main/java/frc/robot/Commands/AmpHandoff.java b/src/main/java/frc/robot/Commands/AmpHandoff.java
index 7c82989..60da463 100644
--- a/src/main/java/frc/robot/Commands/AmpHandoff.java
+++ b/src/main/java/frc/robot/Commands/AmpHandoff.java
@@ -7,7 +7,7 @@ import frc.robot.subsystems.Shooter;
 public class AmpHandoff extends ParallelCommandGroup{
 
     AmpHandoff(Indexer indexer, Shooter shooter){
-        addCommands(indexer.shootNote(() -> 1), shooter.ampHandoff());
+        //addCommands(indexer.shootNote(() -> 1), shooter.ampHandoff());
     }
     
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 5b59115..20a357a 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -90,8 +90,8 @@ public class RobotContainer {
 
         climber = new Climber(shooter::getShooterAngle);
 
-        NamedCommands.registerCommand("Charge Shooter", shooter.angleSpeedsSetpoints(() -> ShooterConstants.kShooterLoadAngle, 1.0, 1.0));
-        NamedCommands.registerCommand("Speake Note Shot", Commands.parallel(shooter.angleSpeedsSetpoints(() -> ShooterConstants.kShooterLoadAngle, 1.0, 1.0), indexer.shootNote(() -> 1.0)));
+        NamedCommands.registerCommand("Charge Shooter 2 Sec", shooter.angleSpeedsSetpoints(() -> ShooterConstants.kShooterLoadAngle, 1.0, 1.0).withTimeout(2.0));
+        NamedCommands.registerCommand("Speaker Note Shot", Commands.parallel(shooter.angleSpeedsSetpoints(() -> ShooterConstants.kShooterLoadAngle, 1.0, 1.0), indexer.shootNote(() -> 1.0)).withTimeout(2.0));
 
         // An example Named Command, doesn't need to remain once we start actually adding real things
         // ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration
@@ -241,6 +241,13 @@ public class RobotContainer {
         */
         driver.start().onTrue(drivetrain.toggleFieldRelativeControl());
 
+        operator.b().whileTrue(Commands.parallel(indexer.shootNote(() -> 1.0), shooter.ampHandoff(() -> driver.getRightTriggerAxis(), () -> {
+            if(operator.getRightTriggerAxis()>0.25){
+                return true;
+            }else{
+                return false;
+            }
+        })));
         
 
         //driver.leftBumper().toggleOnTrue(intake.intakeDownCommand());
@@ -250,7 +257,7 @@ public class RobotContainer {
         
         //operator.a().whileTrue(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterAmpAngle, 0, 0));
 
-        operator.y().whileTrue(climber.setSpeedWithSupplier(operator::getRightTriggerAxis));
+        operator.y().whileTrue(Commands.parallel(climber.setSpeedWithSupplier(operator::getRightTriggerAxis), shooter.climbState()));
 
         driver.a().whileTrue(indexer.shootNote(() -> 1.0));
         
diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java
index 1f23184..3e35e08 100644
--- a/src/main/java/frc/robot/subsystems/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/Shooter.java
@@ -109,8 +109,9 @@ public class Shooter extends SubsystemBase{
             bottomShooter.set(bottomRPM);
     }
 
-    public Command ampHandoff(){
+    public Command ampHandoff(DoubleSupplier shootNoteSpeed, BooleanSupplier ampAngleOrNah){
         return run(() -> {
+            
             shooterPivot.setIdleMode(IdleMode.kBrake);
 
             shooterPivot.setVoltage(
@@ -118,9 +119,9 @@ public class Shooter extends SubsystemBase{
                 shooterPivotFF.calculate(ShooterConstants.kShooterLoadAngle, 0.0));
             
             if(shooterBeamBreak.get() || indexerBeamBreak){
-                angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, 0.5, 0.5);
+                angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, 0.25, 0.25);
             }else{
-                angleAndSpeedControl(ShooterConstants.kShooterAmpAngle, 0, 0);
+                angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, shootNoteSpeed.getAsDouble(), shootNoteSpeed.getAsDouble());
             }
         });
     }