diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5e7b889f..413bd29c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -89,15 +89,6 @@ public static class Controllers { /** Constants for the Drivetrain */ public static class Drive { - /* TODO: MAKE THIS TOGGLEABLE BY DRIVERS - PROBABLY POST-HEARTLAND */ - public static enum DrivePerspectiveOptions { - RobotOriented, - FieldOriented - } - - public static DrivePerspectiveOptions currentDrivePerspective = - DrivePerspectiveOptions.FieldOriented; - /** Empty translation to prevent creating 2 Translation2ds every time the drive train stops. */ public static Translation2d EMPTY_TRANSLATION = new Translation2d(); diff --git a/src/main/java/frc/robot/Controllers.java b/src/main/java/frc/robot/Controllers.java index ad65a374..fcb5e695 100644 --- a/src/main/java/frc/robot/Controllers.java +++ b/src/main/java/frc/robot/Controllers.java @@ -104,20 +104,9 @@ private static void configureDefaultKeybinds() { operatorController.povDown().whileTrue(RobotContainer.runIntakeReverse); operatorController.leftTrigger().whileTrue(RobotContainer.aimToAmp); operatorController.rightTrigger().whileTrue(RobotContainer.fireNote); + pilotController.leftTrigger().toggleOnTrue(RobotContainer.driveFieldOriented); - // pilotController.leftTrigger().toggleOnTrue(RobotContainer.driveFieldOriented); - pilotController.rightBumper().toggleOnTrue(RobotContainer.enterXMode); - switch (Constants.Drive.currentDrivePerspective) { - case RobotOriented: - pilotController.leftBumper().whileTrue(RobotContainer.driveRobotOrientedSprint); - break; - case FieldOriented: - pilotController.leftBumper().whileTrue(RobotContainer.driveFieldOrientedSprint); - break; - default: - throw new IllegalStateException(); - } - /* TODO: angle / velocity steering toggle w/ right trigger (no issue) and boost on left bumper (issue 86) */ + /* TODO: angle / velocity steering toggle w/ right trigger (no issue) */ keymapEntry.setString("Default"); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ce93a347..dc023519 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -33,6 +33,9 @@ import frc.robot.commands.auto.TaxiAuto; import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; import frc.robot.commands.drive.DriveRobot; +import frc.robot.commands.drive.SetDrivePerspectiveFieldOriented; +import frc.robot.commands.drive.SetDrivePerspectiveFieldOrientedHeadingSnapping; +import frc.robot.commands.drive.SetDrivePerspectiveRobotOriented; import frc.robot.commands.drive.ZeroGyro; import frc.robot.commands.mailbox.DeindexNote; import frc.robot.commands.mailbox.DeployMailbox; @@ -222,6 +225,22 @@ public class RobotContainer { /** Singleton instance of the intake {@link StartCamera} for the whole robot. */ public static StartCamera startIntakeCamera = new StartCamera(intakeCamera); + /** Singleton instance of {@link SetDrivePerspectiveFieldOriented} for the whole robot. */ + public static SetDrivePerspectiveFieldOriented setDrivePerspectiveFieldOriented = + new SetDrivePerspectiveFieldOriented(); + + /** + * Singleton instance of {@link SetDrivePerspectiveFieldOrientedHeadingSnapping} for the whole + * robot. + */ + public static SetDrivePerspectiveFieldOrientedHeadingSnapping + setDrivePerspectiveFieldOrientedHeadingSnapping = + new SetDrivePerspectiveFieldOrientedHeadingSnapping(); + + /** Singleton instance of {@link SetDrivePerspectiveRobotOriented} for the whole robot. */ + public static SetDrivePerspectiveRobotOriented setDrivePerspectiveRobotOriented = + new SetDrivePerspectiveRobotOriented(); + /* Autos */ /** Singleton instance of {@link MoveAwayFromAmp} for the whole robot. */ public static MoveAwayFromAmp moveAwayFromAmp = new MoveAwayFromAmp(); @@ -255,19 +274,18 @@ public RobotContainer() { configureAuto(); Shuffleboard.getTab("Driver").add("Clear PDP sticky faults", clearPDPStickyFaults); Shuffleboard.getTab("Driver").add("Zero Gyro", zeroGyro); + Shuffleboard.getTab("Driver") + .add("Set Drive Perspective to Field Oriented", setDrivePerspectiveFieldOriented); + Shuffleboard.getTab("Driver") + .add( + "Set Drive Perspective to Field Oriented Heading Snapping", + setDrivePerspectiveFieldOrientedHeadingSnapping); + Shuffleboard.getTab("Driver") + .add("Set Drive Perspective Robot Oriented", setDrivePerspectiveRobotOriented); startIntakeCamera.schedule(); - switch (Constants.Drive.currentDrivePerspective) { - case RobotOriented: - drive.setDefaultCommand(driveRobotOriented); - break; - case FieldOriented: - drive.setDefaultCommand(driveFieldOriented); - break; - default: - throw new IllegalStateException(); - } + drive.setDefaultCommand(driveFieldOriented); } private void configureAuto() { diff --git a/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveFieldOriented.java b/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveFieldOriented.java new file mode 100644 index 00000000..d8dc305f --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveFieldOriented.java @@ -0,0 +1,49 @@ +// 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. + +/* + * Asimov's Laws: + * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm. + * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law. + * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law. + */ + +package frc.robot.commands.drive; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Controllers; +import frc.robot.RobotContainer; +import frc.robot.subsystems.Drive; + +/** Sets the current drive perspective to field oriented. */ +public class SetDrivePerspectiveFieldOriented extends Command { + /** Creates a new SetDrivePerspectiveFieldOriented. */ + private Drive drive; + + public SetDrivePerspectiveFieldOriented() { + this.drive = RobotContainer.drive; + addRequirements(drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + drive.setDefaultCommand(RobotContainer.driveFieldOriented); + Controllers.pilotController.leftBumper().whileTrue(RobotContainer.driveFieldOrientedSprint); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveFieldOrientedHeadingSnapping.java b/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveFieldOrientedHeadingSnapping.java new file mode 100644 index 00000000..1bb55177 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveFieldOrientedHeadingSnapping.java @@ -0,0 +1,49 @@ +// 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. + +/* + * Asimov's Laws: + * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm. + * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law. + * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law. + */ + +package frc.robot.commands.drive; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Controllers; +import frc.robot.RobotContainer; +import frc.robot.subsystems.Drive; + +/** Sets the current drive perspective to field oriented with heading snapping. */ +public class SetDrivePerspectiveFieldOrientedHeadingSnapping extends Command { + /** Creates a new SetDrivePerspectiveFieldOriented. */ + private Drive drive; + + public SetDrivePerspectiveFieldOrientedHeadingSnapping() { + this.drive = RobotContainer.drive; + addRequirements(drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + drive.setDefaultCommand(RobotContainer.driveFieldOrientedHeadingSnapping); + Controllers.pilotController.leftBumper().whileTrue(RobotContainer.driveFieldOrientedSprint); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveRobotOriented.java b/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveRobotOriented.java new file mode 100644 index 00000000..5397fd26 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveRobotOriented.java @@ -0,0 +1,49 @@ +// 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. + +/* + * Asimov's Laws: + * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm. + * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law. + * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law. + */ + +package frc.robot.commands.drive; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Controllers; +import frc.robot.RobotContainer; +import frc.robot.subsystems.Drive; + +/** Sets the current drive perspective to robot oriented. */ +public class SetDrivePerspectiveRobotOriented extends Command { + /** Creates a new SetDrivePerspectiveFieldOriented. */ + private Drive drive; + + public SetDrivePerspectiveRobotOriented() { + this.drive = RobotContainer.drive; + addRequirements(drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + drive.setDefaultCommand(RobotContainer.driveRobotOriented); + Controllers.pilotController.leftBumper().whileTrue(RobotContainer.driveRobotOrientedSprint); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +}