Almost Ready for Florida!! (im geekin)

src/main/java/frc4388/robot/Constants.java

@@ -168,7 +168,7 @@ public final class Constants {
     public static final class ShooterConstants {
         public static final int LEFT_SHOOTER_ID = 15; // final
         public static final int RIGHT_SHOOTER_ID = 16; // final
-        public static final double SHOOTER_SPEED = 0.35; // final
+        public static final double SHOOTER_SPEED = 0.40; // final
         public static final double SHOOTER_IDLE = 0.35; // final
         public static final double SHOOTER_IDLE_LIMELIGHT = 0.8;
     }

src/main/java/frc4388/robot/RobotContainer.java

@@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
@@ -25,6 +26,7 @@ import frc4388.robot.commands.Swerve.JoystickRecorder;
 import frc4388.robot.commands.Swerve.neoJoystickPlayback;
 import frc4388.robot.commands.Swerve.neoJoystickRecorder;
 import frc4388.robot.commands.Intake.ArmIntakeIn;
+import frc4388.robot.commands.Autos.ArmIntakeInAuto;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.Limelight;
 import frc4388.robot.subsystems.SwerveDrive;
@@ -76,8 +78,8 @@ public class RobotContainer {
     private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter);
 
     private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup(
-        new InstantCommand(() -> m_robotIntake.talonPIDIn())
+        new InstantCommand(() -> m_robotIntake.talonPIDIn()),
-        //new InstantCommand(() -> m_robotShooter.spin())
+        new InstantCommand(() -> m_robotShooter.spin())
     );
 
     // private SequentialCommandGroup outtakeToShootFull = new SequentialCommandGroup(
@@ -112,9 +114,9 @@ public class RobotContainer {
     );
 
     private SequentialCommandGroup ejectToShoot = new SequentialCommandGroup(
-         new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)
+        new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
-        // new WaitCommand(0.75),
+        new WaitCommand(0.75),
-        //new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake)
+        new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake)
     );
 
     private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup(
@@ -126,6 +128,7 @@ public class RobotContainer {
     private Command taxi = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
     private Command startLeftMoveRight = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartLeftMoveRight.txt");
     private Command startRightMoveLeft = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartRightMoveLeft.txt");
+    private Command interrupt = new InstantCommand(() -> {}, m_robotSwerveDrive, m_robotIntake, m_robotShooter);
 
     private SequentialCommandGroup oneNoteStartingSpeaker = new SequentialCommandGroup (
         new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
@@ -155,6 +158,7 @@ public class RobotContainer {
         ejectToShoot.asProxy(),
         taxi.asProxy()
     );
+
     private SequentialCommandGroup twoNoteStartingFromSpeaker = new SequentialCommandGroup(
         new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
         new WaitCommand(1).asProxy(),
@@ -162,26 +166,32 @@ public class RobotContainer {
         new WaitCommand(1).asProxy(),
         new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
         new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
-        intakeToShootStuff.asProxy(),
+        new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive),
-        new WaitCommand(1).asProxy(),
+        new InstantCommand(() -> m_robotIntake.talonPIDIn()),
-        new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt1.txt"),
-        intakeToShoot.asProxy(),
-        new WaitCommand(1).asProxy(),
-        new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt2.txt"),
-        new WaitCommand(0.5).asProxy(),
         new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
-        new WaitCommand(1).asProxy(),
+        new WaitCommand(1.4).asProxy(),
         new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
-        new WaitCommand(1).asProxy(),
+        new WaitCommand(0.5),
         new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
-        new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
+        new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
+        new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")
+        // new WaitCommand(1).asProxy(),
+        // new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt2.txt"),
+        // new WaitCommand(0.5).asProxy(),
+        // new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
+        // new WaitCommand(1).asProxy(),
+        // new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
+        // new WaitCommand(1).asProxy(),
+        // new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
+        // new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
     );
     private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
             .addOption("Taxi Auto", taxi.asProxy())
             .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker)
             .addOption("One Note Auto Starting in Front of Speaker, But Stay", oneNoteStartingSpeakerStationary)
-            .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft)
+            // .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft)
-            .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight)
+            // .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight)
+            .addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker)
             .buildDisplay();
     
     
@@ -222,20 +232,18 @@ public class RobotContainer {
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive));
         
         /* Auto Recording */
-        new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
+        // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
-           .whileTrue(new JoystickRecorder(m_robotSwerveDrive, m_robotShooter, m_robotIntake,
+        //    .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
-                                           () -> getDeadbandedDriverController().getLeftX(),
+        //                                    () -> getDeadbandedDriverController().getLeftX(),
-                                           () -> getDeadbandedDriverController().getLeftY(),
+        //                                    () -> getDeadbandedDriverController().getLeftY(),
-                                           () -> getDeadbandedDriverController().getRightX(),
+        //                                    () -> getDeadbandedDriverController().getRightX(),
-                                           () -> getDeadbandedDriverController().getRightY(),
+        //                                    () -> getDeadbandedDriverController().getRightY(),
-                                           () -> getDeadbandedOperatorController().getLeftBumper(),
+        //                                    "Taxi.txt"))
-                                           () -> getDeadbandedOperatorController().getRightBumper(),
+        //    .onFalse(new InstantCommand());
-                                           "Taxi.txt"))
-           .onFalse(new InstantCommand());
 
-        new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
+        // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
-           .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"))
+        //    .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"))
-           .onFalse(new InstantCommand()); 
+        //    .onFalse(new InstantCommand()); 
 
         // new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
         //    .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
@@ -251,9 +259,9 @@ public class RobotContainer {
         //    .onFalse(new InstantCommand());
 
         // /* Speed */
-        new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
+       new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
-            .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.shiftUp()));
+             .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.shiftUp()));
-         //   .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
+          // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
         
         new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
@@ -301,6 +309,11 @@ public class RobotContainer {
         new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter));
         
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotIntake.talonSpinIntakeMotor(), m_robotIntake))
+            .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake));
+
+
     }
 
    private void configureVirtualButtonBindings() {
@@ -347,9 +360,9 @@ public class RobotContainer {
             .onFalse(turnOffShoot);
 
 
-        new JoystickButton(getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
+        // new JoystickButton(getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
-            .onTrue(i)
+        //     .onTrue(i)
-            .onFalse(new InstantCommand(() -> m_robotIntake.talonPIDIn()));
+        //     .onFalse(new InstantCommand(() -> m_robotIntake.talonPIDIn()));
         
         
         new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON)

src/main/java/frc4388/robot/commands/Autos/ArmIntakeInAuto.java

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.Autos;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc4388.robot.commands.Intake.ArmIntakeIn;
+import frc4388.robot.commands.Swerve.JoystickPlayback;
+import frc4388.robot.subsystems.Intake;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.SwerveDrive;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class ArmIntakeInAuto extends ParallelCommandGroup {
+  private final Intake intake;
+  private final Shooter shooter;
+  private final SwerveDrive swerve;
+  /** Creates a new ArmIntakeInAuto. */
+  public ArmIntakeInAuto(Intake intake, Shooter shooter, SwerveDrive swerve) {
+    // Add your commands in the addCommands() call, e.g.
+    // addCommands(new FooCommand(), new BarCommand());
+    this.intake = intake;
+    this.shooter = shooter;
+    this.swerve = swerve;
+    addCommands(new ArmIntakeIn(intake, shooter), new WaitCommand(0.5).andThen(new JoystickPlayback(swerve, "TwoNotePrt1.txt")));
+  }
+}

src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java

@@ -7,6 +7,7 @@ package frc4388.robot.commands.Intake;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.SwerveDrive;
 
 public class ArmIntakeIn extends Command {
   /** Creates a new ArmIntakeIn. */

src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java

@@ -9,11 +9,11 @@ import java.io.FileNotFoundException;
 import java.util.ArrayList;
 import java.util.Scanner;
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.UtilityStructs.TimedOutput;
 
-public class JoystickPlayback extends Command {
+public class JoystickPlayback extends CommandBase {
   private final SwerveDrive            swerve;
   private       String                 filename;
   private       int                    mult       = 1;
@@ -102,7 +102,7 @@ public class JoystickPlayback extends Command {
         return;
       }
       lastIndex = i;
-    } // Why is this done rather than using the variable counter 
+    }
 
     TimedOutput lastOut = outputs.get(lastIndex - 1);
     TimedOutput out     = outputs.get(lastIndex);
@@ -138,4 +138,4 @@ public class JoystickPlayback extends Command {
   public boolean isFinished() {
     return m_finished;
   }
-}
+}

src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java

@@ -11,67 +11,34 @@ import java.util.ArrayList;
 import java.util.function.Supplier;
 
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc4388.robot.commands.Intake.ArmIntakeIn;
-import frc4388.robot.subsystems.Intake;
-import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.UtilityStructs.TimedOutput;
 
-public class JoystickRecorder extends Command {
+public class JoystickRecorder extends CommandBase {
   public  final SwerveDrive            swerve;
-  public  final Shooter                m_robotShooter;
-  public  final Intake                 m_robotIntake;
 
   public  final Supplier<Double>       leftX;
   public  final Supplier<Double>       leftY;
   public  final Supplier<Double>       rightX;
   public  final Supplier<Double>       rightY;
-  public  final Supplier<Boolean>      OPLB;
-  public  final Supplier<Boolean>      OPRB;
-
-  private Command intakeToShootStuff;
-  private Command intakeToShoot;
-  private Command i;
-
-  private boolean lastOPLB;
-  private boolean lastOPRB;
-
   private       String                 filename;
   public  final ArrayList<TimedOutput> outputs   = new ArrayList<>();
   private       long                   startTime = -1;
 
 
   /** Creates a new JoystickRecorder. */
-  public JoystickRecorder(SwerveDrive swerve, Shooter m_robotShooter, Intake m_robotIntake,
+  public JoystickRecorder(SwerveDrive swerve, Supplier<Double> leftX,  Supplier<Double> leftY,
-                                              Supplier<Double> leftX,  Supplier<Double> leftY,
                                               Supplier<Double> rightX, Supplier<Double> rightY,
-                                              Supplier<Boolean> OPLB,  Supplier<Boolean> OPRB,
                                               String filename)
   {
     this.swerve = swerve;
-    this.m_robotShooter = m_robotShooter;
-    this.m_robotIntake = m_robotIntake;
-
     this.leftX  = leftX;
     this.leftY  = leftY;
     this.rightX = rightX;
     this.rightY = rightY;
-    this.OPLB = OPLB;
-    this.OPRB = OPRB;
     this.filename = filename;
 
-    intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter);
-    intakeToShoot = new SequentialCommandGroup(
-        new InstantCommand(() -> m_robotIntake.talonPIDIn()),
-        new InstantCommand(() -> m_robotShooter.spin())
-    );
-    i = new SequentialCommandGroup(
-        intakeToShootStuff, intakeToShoot
-    );
-
     addRequirements(this.swerve);
   }
 
@@ -93,8 +60,6 @@ public class JoystickRecorder extends Command {
     inputs.leftY       = leftY.get();
     inputs.rightX      = rightX.get();
     inputs.rightY      = rightY.get();
-    inputs.OPLB        = OPLB.get();
-    inputs.OPRB        = OPRB.get();
     inputs.timedOffset = System.currentTimeMillis() - startTime;
 
     outputs.add(inputs);
@@ -102,23 +67,8 @@ public class JoystickRecorder extends Command {
     swerve.driveWithInput(new Translation2d(inputs.leftX,  inputs.leftY),
                           new Translation2d(inputs.rightX, inputs.rightY),
                           true);
-
-    if(lastOPLB != inputs.OPLB && inputs.OPLB == true){
-      m_robotShooter.spin();
-      m_robotIntake.talonHandoff();
-    }else if(lastOPLB != inputs.OPLB && inputs.OPLB == false){
-
-    }
-
-    if(lastOPRB != inputs.OPRB){
-      m_robotShooter.spin();
-      m_robotIntake.talonHandoff();
-    }
     
     System.out.println("RECORDING");
-
-    lastOPLB = inputs.OPLB;
-    lastOPRB = inputs.OPRB;
   }
 
   // Called once the command ends or is interrupted.
@@ -144,4 +94,4 @@ public class JoystickRecorder extends Command {
   public boolean isFinished() {
     return false;
   }
-}
+}