feat: event changes

flamingchickens1540 · Dec 31, 2023 · 8caf7af · 8caf7af
1 parent 0555c37
commit 8caf7af
Show file tree

Hide file tree

Showing 4 changed files with 31 additions and 6 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -224,6 +224,10 @@ private void configureAutoChooser() {
                     new InstantCommand(() -> drive.resetPose()),
                     new InstantCommand(() -> intake.zeroPivotAngle()),
                     new IntakeCommands.IntakeUp(intake)));
+    autoChooser.addOption(
+            "Shoot and Drive",
+            new DriveCommands.ShootAndDrive(drive, intake, shooter, limelight, indexer)
+    );
   }
 
   /**

diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java
@@ -21,9 +21,12 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj2.command.*;
+import frc.robot.commands.ShooterCommands.Shoot;
 import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.indexer.Indexer;
 import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.limelight.Limelight;
+import frc.robot.subsystems.shooter.Shooter;
 
 import java.util.function.DoubleSupplier;
 
@@ -111,14 +114,32 @@ public static class JustinCase extends SequentialCommandGroup {
         public JustinCase(Drive drive, Intake intake){
             addCommands(
                     new InstantCommand(() -> drive.resetPose()),
-                    new InstantCommand(() -> intake.zeroPivotAngle()),
-                    new IntakeCommands.IntakeUp(intake),
+//                    new InstantCommand(() -> intake.zeroPivotAngle()),
                     new ParallelRaceGroup(
-                            new WaitCommand(1),
+//                            new IntakeCommands.IntakeUp(intake),
+                            new WaitCommand(1)
+                    ),
+                    new ParallelRaceGroup(
+                            new WaitCommand(3),
                             joystickDrive(drive, () -> 0.0, () -> -0.5, () -> 0.0)
                     ),
                     joystickDrive(drive, () -> 0.0, () -> 0.0, () -> 0.0)
             );
         }
     }
+
+    public static class ShootAndDrive extends SequentialCommandGroup{
+      public ShootAndDrive(Drive drive, Intake intake, Shooter shooter, Limelight limelight, Indexer indexer){
+          addCommands(
+                  new JustinCase(drive, intake),
+//                  new InstantCommand(() -> indexer.setVoltage(9)),
+                  new Shoot(shooter, limelight),
+                  new WaitCommand(1),
+                  new Shoot(shooter, limelight),
+                  new WaitCommand(1),
+                  new Shoot(shooter, limelight),
+                  new WaitCommand(1)
+          );
+        }
+    }
 }
diff --git a/src/main/java/frc/robot/commands/IntakeCommands.java b/src/main/java/frc/robot/commands/IntakeCommands.java
@@ -33,7 +33,7 @@ public IntakeUp(Intake intake){
         @Override
         public void initialize() {
             intake.setPivotAngle(Rotation2d.fromRotations(5));
-            intake.rollerVoltage(0);
+            intake.rollerVoltage(8);
         }
     }
 

diff --git a/src/main/java/frc/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc/robot/commands/ShooterCommands/Shoot.java
@@ -51,8 +51,8 @@ public void execute() {
             }
             case SPINUP -> {
                 //TODO calculation based on distance;
-                mainRPM = mainRPMLog.get();
-                secondaryRPM = secondRPMLog.get();
+                mainRPM = 6500;
+                secondaryRPM = 0;
 
                 shooter.setMainVelocity(mainRPM);
                 shooter.setSecondaryVelocity(secondaryRPM);