diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f0429f8..3e0325c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,14 +7,20 @@ package frc4388.robot; -import org.opencv.video.Video; - -import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.cscore.VideoMode; -import edu.wpi.first.math.geometry.Translation2d; +// Drive Systems import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.GenericHID; 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; @@ -22,30 +28,28 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc4388.robot.Constants.IntakeConstants; -import frc4388.robot.Constants.OIConstants; -import frc4388.robot.Constants.ShooterConstants; -//import frc4388.robot.commands.Autos.AutoAlign; + +// Autos import frc4388.robot.commands.Autos.PlaybackChooser; 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.ArmIntakeInAuto; +//import frc4388.robot.commands.Autos.AutoAlign; + +// 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; import frc4388.utility.configurable.ConfigurableString; -import frc4388.utility.controller.DeadbandedXboxController; -import frc4388.utility.controller.VirtualController; -import frc4388.utility.controller.XboxController; /** * This class is where the bulk of the robot should be declared. Since @@ -57,10 +61,12 @@ public class RobotContainer { /* RobotMap */ private final RobotMap m_robotMap = new RobotMap(); - + /* Subsystems */ private final LED m_robotLED = new LED(); + private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, @@ -74,14 +80,11 @@ public class RobotContainer { private final Climber m_robotClimber = new Climber(m_robotMap.climbMotor); - //private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); - /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(2); - private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); /* Virtual Controllers */ private final VirtualController m_virtualDriver = new VirtualController(0); @@ -91,23 +94,12 @@ public class RobotContainer { private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter); private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.talonPIDIn()), + 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()) ); - // private SequentialCommandGroup outtakeToShootFull = new SequentialCommandGroup( - // new InstantCommand(() -> m_robotShooter.spin()), - // new InstantCommand(() -> m_robotIntake.handoff()) - // ); - - // private SequentialCommandGroup intakeInToOut = new SequentialCommandGroup( - // new InstantCommand(() -> m_robotIntake.rotateArmOut2(), m_robotIntake), - // new RunCommand(() -> m_robotIntake.limitNote(), m_robotIntake).until(m_robotIntake.getArmFowardLimitState()), - // new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter) - // ); - // ! Teleop Commands @@ -118,7 +110,7 @@ public class RobotContainer { //autoAlign, new InstantCommand(() -> m_robotShooter.spin()), new WaitCommand(3.0), - new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), + new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake), new WaitCommand(3.0), new InstantCommand(() -> m_robotShooter.idle()) // new InstantCommand(() -> autoAlign.reverse()), @@ -134,7 +126,7 @@ public class RobotContainer { private SequentialCommandGroup ejectToShoot = new SequentialCommandGroup( new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), new WaitCommand(0.75), - new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake) + new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake) ); private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup( @@ -144,8 +136,8 @@ public class RobotContainer { private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup( interrupt, - new InstantCommand(() -> m_robotIntake.talonPIDIn(), m_robotIntake), - new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) + new InstantCommand(() -> m_robotIntake.PIDIn(), m_robotIntake), + new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake) ); private SequentialCommandGroup ampShoot = new SequentialCommandGroup( @@ -154,137 +146,24 @@ public class RobotContainer { ); // ! /* Autos */ - private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); //new InstantCommand(); - private Command startLeftMoveRight = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartLeftMoveRight.txt"); - private Command startRightMoveLeft = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartRightMoveLeft.txt"); + // 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, - lastAutoName, // () -> autoplaybackName.get(), + () -> autoplaybackName.get(), // lastAutoName new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, - true, true); - - - //Help Simplify Shooting - // private SequentialCommandGroup pullInArmtoShoot = new SequentialCommandGroup( - // new InstantCommand(() -> m_robotIntake.talonPIDIn()), - // new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - // new WaitCommand(1.4).asProxy(), - // new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), - // new WaitCommand(0.5), - // new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - // new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) - // ); - - private SequentialCommandGroup oneNoteStartingSpeaker = new SequentialCommandGroup ( - 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), - new WaitCommand(1).asProxy(), - new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt") - ); - private SequentialCommandGroup oneNoteStartingSpeakerStationary = new SequentialCommandGroup ( - 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 SequentialCommandGroup oneNoteStartingFromLeft = new SequentialCommandGroup( - startLeftMoveRight.asProxy(), - ejectToShoot.asProxy(), - taxi.asProxy() - ); - private SequentialCommandGroup oneNoteStartingFromRight = new SequentialCommandGroup( - startRightMoveLeft.asProxy(), - ejectToShoot.asProxy(), - taxi.asProxy() - ); - - - private SequentialCommandGroup twoNoteStartingFromSpeaker = new SequentialCommandGroup( - 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), - new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive), - new InstantCommand(() -> m_robotIntake.talonPIDIn()), - new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - new WaitCommand(1.4).asProxy(), - new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), - new WaitCommand(0.5), - new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - 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 SequentialCommandGroup stayTwoNoteStartingFromSpeaker = new SequentialCommandGroup( - 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), - new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive), - new InstantCommand(() -> m_robotIntake.talonPIDIn()), - new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - new WaitCommand(1.4).asProxy(), - new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), - new WaitCommand(0.5), - new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) - ); - - private SequentialCommandGroup threeNoteStartingFromSpeaker = new SequentialCommandGroup( - 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), - new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive), - new InstantCommand(() -> m_robotIntake.talonPIDIn()), - new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - new WaitCommand(1.4).asProxy(), - new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), - new WaitCommand(0.5), - new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), - //? Create Another Parallel Command Group :( - new InstantCommand(() -> m_robotIntake.talonPIDIn()), - new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - new WaitCommand(1.4).asProxy(), - new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), - new WaitCommand(0.5), - new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), - new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt") - ); - - 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(); + 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(); @@ -341,14 +220,14 @@ private void configureButtonBindings() { // ? /* Driver Buttons */ - new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive)); - new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) + DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.BACK_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue())) .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); - new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) + DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.START_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp())) .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); @@ -434,48 +313,48 @@ private void configureButtonBindings() { //? /* Operator Buttons */ - new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.talonPIDIn())) - .onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor())); + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.PIDIn())) + .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); - new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.talonPIDOut())) - .onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor())); + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.PIDOut())) + .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); - new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.talonHandoff())) - .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors())); + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.handoff())) + .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors())); - new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON) .onTrue(emergencyRetract); // Override Intake Position encoder: out new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-55), m_robotIntake)); + .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.talonSetPivotEncoderPosition(-6.2), m_robotIntake)); + .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-6.2), m_robotIntake)); - new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotShooter.spin(0.5), m_robotShooter)) .onFalse(turnOffShoot); - new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) .onTrue(i) - .onFalse(new InstantCommand(() -> m_robotIntake.talonPIDIn())); + .onFalse(new InstantCommand(() -> m_robotIntake.PIDIn())); //spins up shooter, no wind down - new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON) + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON) .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)); - // new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) + // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) // .onTrue(new InstantCommand(() -> m_robotIntake.talonSpinIntakeMotor(), m_robotIntake)) // .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)); - new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) .onTrue(emergencyRetract); new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5) @@ -490,156 +369,36 @@ private void configureButtonBindings() { .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5))); } + + /** + * This method is used to replcate {@link Trigger Triggers} for {@link VirtualController Virtual Controllers}. + * Please use {@link RobotContainer#DualJoystickButton} for standard buttons. + */ + private void configureVirtualButtonBindings() { - private void configureVirtualButtonBindings() { - // ? /* Driver Buttons */ - - new JoystickButton(getVirtualDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive)); - - new JoystickButton(getVirtualDriverController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue())) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); - - new JoystickButton(getVirtualDriverController(), XboxController.START_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp())) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); - - - // * /* D-Pad Stuff */ - // new Trigger(() -> getVirtualDriverController().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(() -> getVirtualDriverController().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(() -> getVirtualDriverController().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(() -> getVirtualDriverController().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()); + /* Notice: the following buttons have not been replicated + * Swerve Drive Slow and Fast mode Gear Shifts : Fast mode is known to cause drift, so we disable that feature in Autoplayback + * Swerve Drive Rotation Gear Shifts : Same reason as Slow and Fast mode. + * Auto Recording controls : We don't want an Null Ouroboros for an auto. + */ - // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, - // () -> getDeadbandedDriverController().getLeftX(), - // () -> getDeadbandedDriverController().getLeftY(), - // () -> getDeadbandedDriverController().getRightX(), - // () -> getDeadbandedDriverController().getRightY(), - // "Taxi.txt")) - // .onFalse(new InstantCommand()); + // ? /* Operator Buttons */ - // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) - // .onFalse(new InstantCommand()); - // ! /* Speed */ - new JoystickButton(getVirtualDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); - - new JoystickButton(getVirtualDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + /* Notice: the following buttons have not been replicated + * Override Intake Position Encoder : It's an emergancy overide, for when the position of intake when the robot boots, the intake is not inside the robot. + * We don't need it in an auto. + * Climbing controls : We don't need to climb in auto. + */ - new Trigger(() -> getVirtualDriverController().getPOV() == 270) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot())); + // new Trigger(() -> getVirtualOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5) + // .onTrue(new InstantCommand(() -> m_robotClimber.climbOut())) + // .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); - new Trigger(() -> getVirtualDriverController().getPOV() == 90) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot())); - - // new JoystickButton(getVirtualDriverController(), XboxController.Y_BUTTON) - // .whileTrue(new InstantCommand(() -> - // m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), - // new Translation2d(0, 0), - // true), m_robotSwerveDrive)); - - - //? /* Operator Buttons */ - - new JoystickButton(getVirtualOperatorController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.talonPIDIn())) - .onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor())); - - new JoystickButton(getVirtualOperatorController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.talonPIDOut())) - .onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor())); - - new JoystickButton(getVirtualOperatorController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.talonHandoff())) - .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors())); - - new JoystickButton(getVirtualOperatorController(), XboxController.B_BUTTON) - .onTrue(emergencyRetract); - - - // Override Intake Position encoder: out - new JoystickButton(getVirtualOperatorController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-55), m_robotIntake)); - - // Override Intake Position encoder: in - new JoystickButton(getVirtualOperatorController(), XboxController.START_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-6.2), m_robotIntake)); - - new JoystickButton(getVirtualOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotShooter.spin(0.5), m_robotShooter)) - .onFalse(turnOffShoot); - - - new JoystickButton(getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(i) - .onFalse(new InstantCommand(() -> m_robotIntake.talonPIDIn())); - - //spins up shooter, no wind down - new JoystickButton(getVirtualOperatorController(), 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)); - - new JoystickButton(getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) - .onTrue(emergencyRetract); - - 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().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))); @@ -657,6 +416,16 @@ public Command getAutonomousCommand() { //return playbackChooser.getCommand(); } + /** + * A button binding for two controllers, preferably an {@link DeadbandedXboxController Xbox Controller} and {@link VirtualController Virtual Xbox Controller} + * @param joystickA A controller + * @param joystickB A controller + * @param buttonNumber The button to bind to + */ + public Trigger DualJoystickButton(GenericHID joystickA, GenericHID joystickB, int buttonNumber) { + return new Trigger(() -> (joystickA.getRawButton(buttonNumber) || joystickB.getRawButton(buttonNumber))); + } + /** * Add your docs here. */ diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index ef03f99..512b84c 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -13,12 +13,8 @@ import com.ctre.phoenix.sensors.WPI_Pigeon2; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; -import com.revrobotics.CANSparkLowLevel; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.motorcontrol.Spark; -import edu.wpi.first.wpilibj.motorcontrol.Talon; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.Constants.ShooterConstants; diff --git a/src/main/java/frc4388/robot/commands/Autos/ArmIntakeInTimeout.java b/src/main/java/frc4388/robot/commands/Autos/ArmIntakeInTimeout.java index 1e1e523..5a40e2a 100644 --- a/src/main/java/frc4388/robot/commands/Autos/ArmIntakeInTimeout.java +++ b/src/main/java/frc4388/robot/commands/Autos/ArmIntakeInTimeout.java @@ -29,23 +29,23 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - robotIntake.talonPIDOut(); - robotIntake.talonSpinIntakeMotor(); + robotIntake.PIDOut(); + robotIntake.spinIntakeMotor(); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { if(interrupted) { - robotIntake.talonPIDIn(); - robotIntake.talonStopIntakeMotors(); + robotIntake.PIDIn(); + robotIntake.stopIntakeMotors(); } } // Returns true when the command should end. @Override public boolean isFinished() { - return robotIntake.getTalonIntakeLimitSwitchState(); + return robotIntake.getIntakeLimitSwitchState(); // if(!(!robotIntake.getTalonIntakeLimitSwitchState() != !false) && ((-1.0 / 0.0) == (-2.0 / 0.0))) // { // return !true==true; diff --git a/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java b/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java index 312325e..9b46468 100644 --- a/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java @@ -43,7 +43,7 @@ public double getError() { public void runWithOutput(double output) { double out2 = MathUtil.clamp(output / 40, -59, 0); if (Math.abs(getError()) < 3) out2 = 0; - intake.talonPIDPosition(out2); + intake.PIDPosition(out2); } } diff --git a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java index 78c558b..42f9dfc 100644 --- a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java +++ b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java @@ -29,8 +29,8 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - robotIntake.talonPIDOut(); - robotIntake.talonSpinIntakeMotor(); + robotIntake.PIDOut(); + robotIntake.spinIntakeMotor(); } // Called once the command ends or is interrupted. @@ -40,7 +40,7 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return robotIntake.getTalonIntakeLimitSwitchState(); + return robotIntake.getIntakeLimitSwitchState(); // if(!(!robotIntake.getTalonIntakeLimitSwitchState() != !false) && ((-1.0 / 0.0) == (-2.0 / 0.0))) // { // return !true==true; diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index abd12fc..7d7294c 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -4,116 +4,30 @@ package frc4388.robot.subsystems; -import java.util.function.BooleanSupplier; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; -import com.ctre.phoenix.motorcontrol.can.TalonSRXPIDSetConfiguration; -import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.ForwardLimitTypeValue; -import com.ctre.phoenix6.signals.ForwardLimitValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.ReverseLimitTypeValue; -import com.ctre.phoenix6.signals.ReverseLimitValue; -import com.revrobotics.CANSparkBase; -import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkLimitSwitch; -import com.revrobotics.SparkPIDController; -import com.revrobotics.RelativeEncoder; -import edu.wpi.first.wpilibj.CAN; -import edu.wpi.first.wpilibj.motorcontrol.Spark; -import edu.wpi.first.wpilibj.motorcontrol.Talon; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants; import frc4388.robot.Constants.IntakeConstants; -import frc4388.robot.commands.PID; import frc4388.utility.Gains; -import frc4388.utility.configurable.ConfigurableDouble; public class Intake extends SubsystemBase { - - //NEO - private CANSparkMax intakeMotor; - private CANSparkMax pivot; - private SparkPIDController m_spedController; - private SparkLimitSwitch forwardLimit; - private SparkLimitSwitch reverseLimit; - private SparkLimitSwitch intakeforwardLimit; - private SparkLimitSwitch intakereverseLimit; - - //Talon - private TalonFX talonIntake; - private TalonFX talonPivot; - private CANcoder encoder; - - private boolean r; - - private HardwareLimitSwitchConfigs l; - - TalonFXConfiguration doodooController = new TalonFXConfiguration(); - + private TalonFX intakeMotor; + private TalonFX pivotMotor; public static Gains armGains = IntakeConstants.ArmPID.INTAKE_GAINS; - private ConfigurableDouble outtakeSpeed = new ConfigurableDouble("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); - private BooleanSupplier sup = () -> true; - private BooleanSupplier dup = () -> false; - - private double smartDashboardOuttakeValue; /** Creates a new Intake. */ - //For NEO - // public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) { - // this.intakeMotor = intakeMotor; - // this.pivot = pivot; - - // pivot.restoreFactoryDefaults(); - // //pivot.setInverted(true); - - // forwardLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); - // reverseLimit = pivot.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); - // forwardLimit.enableLimitSwitch(true); - // reverseLimit.enableLimitSwitch(true); - - // intakeMotor.restoreFactoryDefaults(); - - // intakeforwardLimit = intakeMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); - // intakereverseLimit = intakeMotor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); - // intakeforwardLimit.enableLimitSwitch(true); - // intakereverseLimit.enableLimitSwitch(false); - - // //Arm PID - // m_spedController = pivot.getPIDController(); - // m_spedController.setP(armGains.kP); - // m_spedController.setI(armGains.kI); - // m_spedController.setD(armGains.kD); - - // SmartDashboard.putNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); - // } - - //For Talon - public Intake(TalonFX talonIntake, TalonFX talonPivot) { - this.talonIntake = talonIntake; - this.talonPivot = talonPivot; + public Intake(TalonFX intakeMotor, TalonFX pivotMotor) { + this.intakeMotor = intakeMotor; + this.pivotMotor = pivotMotor; - talonIntake.getConfigurator().apply(new TalonFXConfiguration()); - talonPivot.getConfigurator().apply(new TalonFXConfiguration()); + intakeMotor.getConfigurator().apply(new TalonFXConfiguration()); + pivotMotor.getConfigurator().apply(new TalonFXConfiguration()); - talonIntake.setNeutralMode(NeutralModeValue.Brake); - talonPivot.setNeutralMode(NeutralModeValue.Brake); - - // talonPivot.getConfigurator().apply(new HardwareLimitSwitchConfigs()); - // talonIntake.getConfigurator().apply(new HardwareLimitSwitchConfigs()); - - - - // doodooController.Slot0.kP = armGains.kP; - // doodooController.Slot1.kI = armGains.kI; - // doodooController.Slot2.kD = armGains.kD; + intakeMotor.setNeutralMode(NeutralModeValue.Brake); + pivotMotor.setNeutralMode(NeutralModeValue.Brake); // in init function, set slot 0 gains var slot0Configs = new Slot0Configs(); @@ -121,222 +35,76 @@ public Intake(TalonFX talonIntake, TalonFX talonPivot) { slot0Configs.kI = 0.0; // no output for integrated error slot0Configs.kD = 0.21; // A velocity of 1 rps results in 0.1 V output - talonPivot.getConfigurator().apply(slot0Configs); - - + pivotMotor.getConfigurator().apply(slot0Configs); } // ! Talon Methods - public void talonPIDIn() { - PositionVoltage request = new PositionVoltage(-53); - talonPivot.setControl(request.withPosition(0)); + public void PIDIn() { + PIDPosition(0); } - public void talonPIDOut() { - PositionVoltage request = new PositionVoltage(0); - talonPivot.setControl(request.withPosition(-53)); + public void PIDOut() { + PIDPosition(-53); } - public void talonPIDPosition(double out2) { - PositionVoltage request = new PositionVoltage(out2); - talonPivot.setControl(request); + public void PIDPosition(double pos) { + PositionVoltage request = new PositionVoltage(pos); + pivotMotor.setControl(request); } - public void talonHandoff() { - talonIntake.set(-outtakeSpeed.get()); + public void handoff() { + intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); } - public void talonSpinIntakeMotor() { - talonIntake.set(IntakeConstants.INTAKE_SPEED); + public void spinIntakeMotor() { + intakeMotor.set(IntakeConstants.INTAKE_SPEED); } - public void talonSpinIntakeMotor(double speed) { - talonIntake.set(speed); + public void spinIntakeMotor(double speed) { + intakeMotor.set(speed); } - public boolean getTalonIntakeLimitSwitchState() { - if(r = talonIntake.getForwardLimit().getValue().value == 0) { - return true; - } - return false; + public boolean getIntakeLimitSwitchState() { + return intakeMotor.getForwardLimit().getValue().value == 0; } - public void talonSetPivotEncoderPosition(double val) { - talonPivot.setPosition(val); + public void setPivotEncoderPosition(double val) { + pivotMotor.setPosition(val); } - public void talonStopIntakeMotors() { - talonIntake.set(0); + public void stopIntakeMotors() { + intakeMotor.set(0); + } + + public void stopArmMotor() { + pivotMotor.set(0); } - public void talonStopArmMotor() { - talonPivot.set(0); + public void stop() { + intakeMotor.set(0); + pivotMotor.set(0); } public double getArmPos() { - return talonPivot.getPosition().getValue(); + return pivotMotor.getPosition().getValue(); } public void resetArmPosition() { - if(getTalonIntakeLimitSwitchState()){ + if (getIntakeLimitSwitchState()) { // talonPivot.setPosition(0); } } public void ampPosition() { - PositionVoltage request = new PositionVoltage(-0); - talonPivot.setControl(request.withPosition(-59)); //TODO: Find actual value + PIDPosition(-59); //TODO: Find actual value } public void ampOuttake(double speed) { - talonSpinIntakeMotor(speed); + spinIntakeMotor(speed); } - - // ! NEO Methods - //hanoff - // public void spinIntakeMotor() { - // intakeMotor.set(IntakeConstants.INTAKE_SPEED); - // } - - // //Rotate robot in for handoff - // public void rotateArmIn() { - // pivot.set(IntakeConstants.PIVOT_SPEED); - // } - - // //Rotates robot out for intake - // public void rotateArmOut() { - // pivot.set(-IntakeConstants.PIVOT_SPEED); - - // } - - // public void pidIn() { - // m_spedController.setReference(2.5, CANSparkMax.ControlType.kPosition); - // //SmartDashboard.putNumber("Velocity Output", pivot.getEncoder().getVelocity()); - // } - - // public void pidOut() { - // m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition); - // } - - // public void limitNote() { - // if (intakeforwardLimit.isPressed()) { - // rotateArmIn2(); - // } else { - // spinIntakeMotor(); - // } - // } - - // public void rotateArmOut2() { - // if(reverseLimit.isPressed()){ - // stopArmMotor(); - // } else { - // pidOut(); - // } - // } - - // public void rotateArmIn2() { - // if(forwardLimit.isPressed()){ - // stopArmMotor(); - // } else { - // pidIn(); - // } - // } - - // public void handoff() { - // intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); - // } - - // public void handoff2() { - // if(intakeforwardLimit.isPressed()) { - // intakeMotor.set(-smartDashboardOuttakeValue); - // } else { - // intakeMotor.set(-smartDashboardOuttakeValue); - // } - // } - - // public void stopIntakeMotors() { - // intakeMotor.set(0); - // } - - // public void stopArmMotor() { - // pivot.set(0); - // } - - // public RelativeEncoder getEncoder() { - // return pivot.getEncoder(); - // } - - // public boolean getForwardLimitSwitchState() { - // return forwardLimit.isPressed(); - // } - - // public boolean getReverseLimitSwitchState() { - // return reverseLimit.isPressed(); - // } - - // public boolean getIntakeLimitSwtichState() { - // return intakeforwardLimit.isPressed(); - // } - - // public void setVoltage(double voltage) { - // pivot.setVoltage(voltage); - // } - - // public double getVelocity() { - // return pivot.getEncoder().getVelocity(); - // } - - // public void setPivotEncoderPosition(int val) { - // pivot.getEncoder().setPosition(val); - // } - - // public void resetPosition() { - // if(forwardLimit.isPressed()) { - // setPivotEncoderPosition(0); - // } - // } - - // public double getPos() { - // return pivot.getEncoder().getPosition(); - // } - - // public double getIntakeVelocity() { - // return intakeMotor.getEncoder().getVelocity(); - // } - - // public void rotateArm() { - - // } - - // public BooleanSupplier getArmFowardLimitState() { - // if(forwardLimit.isPressed()) { - // return sup; - // } else { - // return dup; - // } - // } - - // public void changeIntakeNeutralState() { - // if(forwardLimit.isPressed()) { - // intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast); - // } - // } - @Override public void periodic() { - // This method will be called once per scheduler run - // SmartDashboard.putNumber("Vel Output", getVelocity()); - // SmartDashboard.putNumber("Position", getPos()); - // resetPosition(); - // changeIntakeNeutralState(); - resetArmPosition(); - - // SmartDashboard.putNumber("Pivot Position", getArmPos()); - - //smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); - - //SmartDashboard.putBoolean("Limit Switch State", getTalonIntakeLimitSwitchState()); } } diff --git a/src/main/java/frc4388/utility/DualJoystickButton.java b/src/main/java/frc4388/utility/DualJoystickButton.java new file mode 100644 index 0000000..e4ef5ed --- /dev/null +++ b/src/main/java/frc4388/utility/DualJoystickButton.java @@ -0,0 +1,22 @@ +package frc4388.utility; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj2.command.button.Trigger; + + +/** + * A button binding for two controllers, preferably an {@link frc4388.utility.controller.DeadbandedXboxController Xbox Controller} and {@link frc4388.utility.controller.VirtualController Virtual Xbox Controller} + * @author Zachary Wilke + */ +public class DualJoystickButton extends Trigger { + + /** + * Creates an Button binding on two controllers + * @param joystickA A controller + * @param joystickB A controller + * @param buttonNumber The button to bind to + */ + public DualJoystickButton(GenericHID joystickA, GenericHID joystickB, int buttonNumber) { + super(() -> (joystickA.getRawButton(buttonNumber) || joystickB.getRawButton(buttonNumber))); + } +}