Cleanup and re-enable auto recording

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

@@ -11,46 +11,33 @@ package frc4388.robot;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import frc4388.utility.controller.XboxController;
 import frc4388.utility.controller.DeadbandedXboxController;
 import frc4388.robot.Constants.OIConstants;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
 
 // Commands
 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;
-import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
 
 // Autos
-import frc4388.robot.commands.Autos.PlaybackChooser;
 import frc4388.robot.commands.Intake.ArmIntakeIn;
-import frc4388.robot.commands.Swerve.JoystickPlayback;
-import frc4388.robot.commands.Swerve.JoystickRecorder;
 import frc4388.utility.controller.VirtualController;
 import frc4388.robot.commands.Swerve.neoJoystickPlayback;
 import frc4388.robot.commands.Swerve.neoJoystickRecorder;
-// import frc4388.robot.commands.Intake.ArmIntakeIn;
+
-//import frc4388.robot.commands.Autos.AutoAlign;
+// Subsystems
 import frc4388.robot.subsystems.Climber;
 import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.Limelight;
 import frc4388.robot.subsystems.Shooter;
-// Subsystems
 // import frc4388.robot.subsystems.LED;
 // import frc4388.robot.subsystems.Limelight;
 import frc4388.robot.subsystems.SwerveDrive;
-// import frc4388.robot.subsystems.Shooter;
-// import frc4388.robot.subsystems.Climber;
-// import frc4388.robot.subsystems.Intake;
 
 // Utilites
 import frc4388.utility.DeferredBlock;
@@ -98,28 +85,13 @@ public class RobotContainer {
     private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter);
     private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter);
 
+    // ! Teleop Commands
+
     private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup(
         new InstantCommand(() -> m_robotIntake.PIDIn()),
         new InstantCommand(() -> m_robotShooter.idle())
-        // new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 1.0)).andThen(new WaitCommand(0.2)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 0.0))),
-        // new InstantCommand(() -> m_robotShooter.spin())
     );
-
+    
-
-    // ! Teleop Commands
-    private SequentialCommandGroup autoShoot = new SequentialCommandGroup(
-        // MoveToSpeaker,
-        //autoAlign,
-        new InstantCommand(() -> m_robotShooter.spin()),
-        new WaitCommand(3.0),
-        new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake),
-        new WaitCommand(3.0),
-        new InstantCommand(() -> m_robotShooter.idle())
-        // new InstantCommand(() -> autoAlign.reverse()),
-       // autoAlign.asProxy()
-    );
-
-
     private SequentialCommandGroup i = new SequentialCommandGroup(
         intakeToShootStuff, intakeToShoot, 
         new InstantCommand(() -> m_robotShooter.idle())
@@ -148,66 +120,38 @@ public class RobotContainer {
     );
 
     // ! /*  Autos */
-    // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
     private String lastAutoName = "final_red_center_4note_taxi.auto";
     private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName);
     private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, 
     () -> autoplaybackName.get(), // lastAutoName
            new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
            true, false);
-
+ 
-    // private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
-    //         .addOption("Taxi Auto", taxi.asProxy())
-    //         .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker.asProxy())
-    //         .addOption("Stay One Note Auto Starting in Front of Speaker", oneNoteStartingSpeakerStationary.asProxy())
-    //         // .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft.asProxy())
-    //         // .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight.asProxy())
-    //         .addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker.asProxy())
-    //         .addOption("Stay Two Note Starting in Front of Speaker", stayTwoNoteStartingFromSpeaker.asProxy())
-    //         .buildDisplay();    
-
-    // private SequentialCommandGroup autoShoot = new SequentialCommandGroup(
-    //     // MoveToSpeaker,
-    //     //autoAlign,
-    //     new InstantCommand(() -> m_robotShooter.spin()),
-    //     new WaitCommand(3.0),
-    //     new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake),
-    //     new WaitCommand(3.0),
-    //     new InstantCommand(() -> m_robotShooter.idle())
-    //     // new InstantCommand(() -> autoAlign.reverse()),
-    //    // autoAlign.asProxy()
-    // );
-
-
-    
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
     public RobotContainer() {
         configureButtonBindings();        
-        // configureVirtualButtonBindings();
+        configureVirtualButtonBindings();
-        // new Trigger(() -> autoplaybackName.get().equals(lastAutoName)).onTrue(new InstantCommand(() -> changeAuto()));
+        new DeferredBlock(() -> m_robotSwerveDrive.resetGyro());
-        // new DeferredBlock(() -> m_robotSwerveDrive.resetGyro());
+        DriverStation.silenceJoystickConnectionWarning(true);
+        // CameraServer.startAutomaticCapture();
 
+        /* Default Commands */
+        // ! Swerve Drive Default Command (Regular Rotation)
+        // drives the robot with a two-axis input from the driver controller
+        m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
+            m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
+                                            getDeadbandedDriverController().getRight(),
+                                true);
+        }, m_robotSwerveDrive)
+        .withName("SwerveDrive DefaultCommand"));
 
-        //     DriverStation.silenceJoystickConnectionWarning(true);
+        // ! Swerve Drive One Module Test
-        //     // CameraServer.startAutomaticCapture();
-
-        //     /* Default Commands */
-        //     // drives the robot with a two-axis input from the driver controller
-            // ! Swerve Drive Default Command (Regular Rotation)
-
         // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
         //     m_robotMap.rightFront.go(getDeadbandedDriverController().getLeft());
         // }
-        // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
-        //     m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
-        //                                     getDeadbandedDriverController().getRight(),
-        //                         true);
-        // }, m_robotSwerveDrive)
-        // .withName("SwerveDrive DefaultCommand"));
-        // m_robotSwerveDrive.setToSlow();
 
         // ! Swerve Drive Default Command (Orientation Rotation)
         // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
@@ -215,7 +159,10 @@ public class RobotContainer {
         //                                                  getDeadbandedDriverController().getRightX(), 
         //                                                  getDeadbandedDriverController().getRightY(), 
         //                                                  true);
-        // }, m_robotSwerveDrive));
+        // }, m_robotSwerveDrive))
+        // .withName("SwerveDrive OrientationCommand"));
+        // continually sends updates to the Blinkin LED controller to keep the lights on
+        // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
 
         m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
             m_robotSwerveDrive.driveWithInput(
@@ -226,18 +173,9 @@ public class RobotContainer {
 
 
 
-        // .withName("SwerveDrive OrientationCommand"));
-        // continually sends updates to the Blinkin LED controller to keep the lights on
-        // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
 
     }
 
-    // private void changeAuto() {
-    //     autoPlayback.unloadAuto();
-    //     autoPlayback.loadAuto();
-    //     lastAutoName = autoplaybackName.get();
-    //     System.out.println("AUTO: Changed auto to; `" + lastAutoName + "`");
-    // }
     /**
      * Use this method to define your button->command mappings. Buttons can be
      * created by instantiating a {@link GenericHID} or one of its subclasses
@@ -250,66 +188,11 @@ public class RobotContainer {
 
         DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive));
-
+            
-        // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.BACK_BUTTON)
-        //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue()))
-        //     .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180()));
-
-        // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.START_BUTTON)
-        //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp()))
-        //     .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180()));
-
-       // *  /* D-Pad Stuff */
-    //    new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9)
-    //         .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1),
-    //                                                                            new Translation2d(0, 0),
-    //                                                                            true)))
-    //         .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), 
-    //                                                                             new Translation2d(0, 0), 
-    //                                                                             true)));
-
-    //    new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > -0.9)
-    //         .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, -1),
-    //                                                                            new Translation2d(0, 0),
-    //                                                                            true)))
-    //         .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), 
-    //                                                                             new Translation2d(0, 0), 
-    //                                                                             true)));
-
-    //    new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > 0.9)
-    //         .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(1, 0),
-    //                                                                            new Translation2d(0, 0),
-    //                                                                            true)))
-    //         .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), 
-    //                                                                             new Translation2d(0, 0), 
-    //                                                                             true)));
-
-    //    new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > -0.9)
-    //         .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(-1, 0),
-    //                                                                            new Translation2d(0, 0),
-    //                                                                            true)))
-    //         .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), 
-    //                                                                             new Translation2d(0, 0), 
-                                                                                // true)));
-        
-        // ! /* Auto Recording */
-        // new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
-        //    .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
-        //                 new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
-        //                                    () -> autoplaybackName.get()))
-        //    .onFalse(new InstantCommand());
-        
-        // new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON)
-        //    .onTrue(new neoJoystickPlayback(m_robotSwerveDrive,
-        //    () -> autoplaybackName.get(),
-        //    new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
-        //    true, false))
-        //    .onFalse(new InstantCommand());
-
         // ! /* Speed */
         new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
             .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.setToTurbo()))
-          .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.setToSlow()));
@@ -319,15 +202,14 @@ public class RobotContainer {
 
         new Trigger(() -> getDeadbandedDriverController().getPOV() == 90)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot()));
-        
+          
-        
+            
-        
+        // ?  /* Operator Buttons */
-       //?  /* Operator Buttons */
+            
-
         DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotIntake.PIDIn()))
+        .onTrue(new InstantCommand(() -> m_robotIntake.PIDIn()))
             .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
-
+            
         DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotIntake.PIDOut()))
             .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
@@ -343,7 +225,7 @@ public class RobotContainer {
         // Override Intake Position encoder: out
         new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-55), m_robotIntake));
-
+            
         // Override Intake Position encoder: in
         new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-6.2), m_robotIntake));
@@ -351,13 +233,13 @@ public class RobotContainer {
         DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotShooter.spin(0.5), m_robotShooter))
             .onFalse(turnOffShoot);
-
+            
 
         DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
             .onTrue(i)
             .onFalse(new InstantCommand(() -> m_robotIntake.PIDIn()));
-        
+            
-        //spins up shooter, no wind down
+        // Spins up shooter, no wind down
         DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter));
 
@@ -374,7 +256,22 @@ public class RobotContainer {
 
         new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
             .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5)));
+        
+        // ? /* Programer Buttons (Controller 3)*/
 
+        // * /* Auto Recording */
+        new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
+            .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
+                        new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
+                                            () -> autoplaybackName.get()))
+            .onFalse(new InstantCommand());
+        
+        new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON)
+            .onTrue(new neoJoystickPlayback(m_robotSwerveDrive,
+            () -> autoplaybackName.get(),
+            new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
+            true, false))
+            .onFalse(new InstantCommand());
     }
     
     /**
@@ -398,17 +295,9 @@ public class RobotContainer {
          *                                    We don't need it in an auto.
          * Climbing controls                : We don't need to climb in auto.
          */
+        
+         // ? Notice: the Programer Buttons are not to be replicated because they are designed for debuging the robot, and do not need to be replicated in auto.
 
-        // new Trigger(() -> getVirtualOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5)
-        //     .onTrue(new InstantCommand(() -> m_robotClimber.climbOut()))
-        //     .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
-
-        // new Trigger(() -> getVirtualOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5)
-        //     .onTrue(new InstantCommand(() -> m_robotClimber.climbIn()))
-        //     .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
-
-        new Trigger(() -> getVirtualOperatorController().getPOV() == 0)
-            .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5)));
     }
 
     /**
@@ -418,9 +307,9 @@ public class RobotContainer {
      */
     public Command getAutonomousCommand() {
         //no auto
-        // return autoPlayback;
+        return autoPlayback;
         //return playbackChooser.getCommand();
-        return new Command() {};
+        // return new Command() {};
     }
 
     /**
@@ -433,9 +322,6 @@ public class RobotContainer {
         return new Trigger(() -> (joystickA.getRawButton(buttonNumber) || joystickB.getRawButton(buttonNumber)));
     }
 
-    /**
-     * Add your docs here.
-     */
     public DeadbandedXboxController getDeadbandedDriverController() {
         return this.m_driverXbox;
     }

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

@@ -7,7 +7,6 @@ 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/subsystems/SwerveDrive.java

@@ -37,7 +37,7 @@ public class  SwerveDrive extends SubsystemBase {
   private RobotGyro gyro;
 
   public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // * slow by default
-  public double rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
+  public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
   public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
   
   public double rotTarget = 0.0;