diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c6164f0..8c2a9a7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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(); - // new Trigger(() -> autoplaybackName.get().equals(lastAutoName)).onTrue(new InstantCommand(() -> changeAuto())); - // new DeferredBlock(() -> m_robotSwerveDrive.resetGyro()); + configureVirtualButtonBindings(); + 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); - // // CameraServer.startAutomaticCapture(); - - // /* Default Commands */ - // // drives the robot with a two-axis input from the driver controller - // ! Swerve Drive Default Command (Regular Rotation) - + // ! Swerve Drive One Module Test // 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; } diff --git a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java index 42f9dfc..5fb161a 100644 --- a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java +++ b/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. */ diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 243093e..70f05b6 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/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;