From 0e308e298628647889b03d8d28496f23a764f173 Mon Sep 17 00:00:00 2001
From: Tyler-J42 <tyler.jacques00@gmail.com>
Date: Sat, 24 Feb 2024 03:48:55 -0500
Subject: [PATCH] shooter + amp handoff sequences advancing

---
 src/main/java/frc/robot/Commands/AmpHandoff.java  | 13 +++++++++++++
 src/main/java/frc/robot/Commands/SpeakerShot.java | 13 +++++++++++++
 src/main/java/frc/robot/RobotContainer.java       |  1 +
 src/main/java/frc/robot/subsystems/Indexer.java   | 12 +++++++++++-
 src/main/java/frc/robot/subsystems/Shooter.java   |  7 ++++++-
 5 files changed, 44 insertions(+), 2 deletions(-)
 create mode 100644 src/main/java/frc/robot/Commands/AmpHandoff.java
 create mode 100644 src/main/java/frc/robot/Commands/SpeakerShot.java

diff --git a/src/main/java/frc/robot/Commands/AmpHandoff.java b/src/main/java/frc/robot/Commands/AmpHandoff.java
new file mode 100644
index 0000000..fe261ec
--- /dev/null
+++ b/src/main/java/frc/robot/Commands/AmpHandoff.java
@@ -0,0 +1,13 @@
+package frc.robot.Commands;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import frc.robot.subsystems.Indexer;
+import frc.robot.subsystems.Shooter;
+
+public class AmpHandoff extends ParallelCommandGroup{
+
+    AmpHandoff(Indexer indexer, Shooter shooter){
+        addCommands(indexer.shootNote(null), shooter.ampHandoff());
+    }
+    
+}
diff --git a/src/main/java/frc/robot/Commands/SpeakerShot.java b/src/main/java/frc/robot/Commands/SpeakerShot.java
new file mode 100644
index 0000000..222de7c
--- /dev/null
+++ b/src/main/java/frc/robot/Commands/SpeakerShot.java
@@ -0,0 +1,13 @@
+package frc.robot.Commands;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import frc.robot.constants.ShooterConstants;
+import frc.robot.subsystems.Indexer;
+import frc.robot.subsystems.Shooter;
+
+public class SpeakerShot extends ParallelCommandGroup{
+    
+    SpeakerShot(Indexer indexer, Shooter shooter){
+        addCommands(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0, 0), indexer.shootNote(() -> 1.0));
+    }
+}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 182028f..29ce5fc 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java
index 177eb23..095c0e6 100644
--- a/src/main/java/frc/robot/subsystems/Indexer.java
+++ b/src/main/java/frc/robot/subsystems/Indexer.java
@@ -31,13 +31,23 @@ public class Indexer extends SubsystemBase{
     public Command autoIndexing(){
      return run(() -> {
         if(!indexerBeamBreak.get()){
-            indexerMotor.set(0.5);
+            indexerMotor.set(0.75);
         }else if(indexerBeamBreak.get()){
             indexerMotor.set(0.0);
         }
 
      });
     }
+
+    public Command advanceNote(){
+        return run(() -> {
+            if(indexerBeamBreak.get()){
+                indexerMotor.set(0.75);
+            }else{
+                indexerMotor.set(0.75);
+            }
+        });
+    }
     
     public Command shootNote(DoubleSupplier indexerSpeed){
         return run(() -> {
diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java
index c73c6c8..b992a88 100644
--- a/src/main/java/frc/robot/subsystems/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/Shooter.java
@@ -36,6 +36,8 @@ public class Shooter extends SubsystemBase{
 
     private DigitalInput shooterBeamBreak;
 
+    private boolean indexerBeamBreak;
+
     public Shooter(BooleanSupplier indexerBeamBreak){
         topShooter = new CANSparkMax(ShooterConstants.kTopShooterID, MotorType.kBrushless);
         bottomShooter = new CANSparkMax(ShooterConstants.kBottomShooterID, MotorType.kBrushless);
@@ -58,6 +60,9 @@ public class Shooter extends SubsystemBase{
 
         shooterBeamBreak = new DigitalInput(ShooterConstants.kShooterBeamBreakChannel);
 
+        topShooter.setIdleMode(IdleMode.kBrake);
+        bottomShooter.setIdleMode(IdleMode.kCoast);
+
         bottomShooter.burnFlash();
         shooterPivot.burnFlash();
         topShooter.burnFlash();
@@ -105,7 +110,7 @@ public class Shooter extends SubsystemBase{
                 shooterPivotPID.calculate(pivotEncoder.getDistance(), ShooterConstants.kShooterLoadAngle) + 
                 shooterPivotFF.calculate(ShooterConstants.kShooterLoadAngle, 0.0));
             
-            if(shooterBeamBreak.get() || shooterBeamBreak.get()){
+            if(shooterBeamBreak.get() || indexerBeamBreak){
                 angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, 1000.0, 1000.0);
             }else{
                 angleAndSpeedControl(ShooterConstants.kShooterAmpAngle, 0, 0);