diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3ad5583..d03da23 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -208,9 +208,18 @@ public static final class Setpoints { new ElevatorState(MIN_EXTENSION_INCHES, 75); public static final ElevatorState GROUND_INTAKE_CUBE = new ElevatorState(MIN_EXTENSION_INCHES, 85); - public static final ElevatorState SCORE_HIGH = new ElevatorState(MAX_EXTENSION_INCHES, 20); - public static final ElevatorState SCORE_MID = new ElevatorState(MAX_EXTENSION_INCHES / 2, 40); - public static final ElevatorState SCORE_LOW = new ElevatorState(MIN_EXTENSION_INCHES, 60); + public static final ElevatorState SCORE_HIGH_CONE = + new ElevatorState(MAX_EXTENSION_INCHES, 20); + public static final ElevatorState SCORE_HIGH_CUBE = + new ElevatorState(MAX_EXTENSION_INCHES, 20); + public static final ElevatorState SCORE_MID_CONE = + new ElevatorState(MAX_EXTENSION_INCHES / 2, 40); + public static final ElevatorState SCORE_MID_CUBE = + new ElevatorState(MAX_EXTENSION_INCHES / 2, 40); + public static final ElevatorState SCORE_LOW_CONE = + new ElevatorState(MIN_EXTENSION_INCHES, 60); + public static final ElevatorState SCORE_LOW_CUBE = + new ElevatorState(MIN_EXTENSION_INCHES, 60); } public static final double MAX_EXTENSION_INCHES = 54; @@ -261,27 +270,27 @@ public static final class Ports { Map.of( NodeType.CONE.atHeight(Height.HIGH), List.of( - new ScoreStep(Elevator.Setpoints.SCORE_HIGH).canWaitHere(), - new ScoreStep(Elevator.Setpoints.SCORE_HIGH, IntakeSubsystem.Modes.OUTTAKE)), + new ScoreStep(Elevator.Setpoints.SCORE_HIGH_CONE).canWaitHere(), + new ScoreStep(Elevator.Setpoints.SCORE_HIGH_CONE, IntakeSubsystem.Modes.OUTTAKE)), NodeType.CONE.atHeight(Height.MID), List.of( - new ScoreStep(Elevator.Setpoints.SCORE_MID).canWaitHere(), - new ScoreStep(Elevator.Setpoints.SCORE_MID, IntakeSubsystem.Modes.OUTTAKE)), + new ScoreStep(Elevator.Setpoints.SCORE_MID_CONE).canWaitHere(), + new ScoreStep(Elevator.Setpoints.SCORE_MID_CONE, IntakeSubsystem.Modes.OUTTAKE)), NodeType.CONE.atHeight(Height.LOW), List.of( - new ScoreStep(Elevator.Setpoints.SCORE_LOW).canWaitHere(), + new ScoreStep(Elevator.Setpoints.SCORE_LOW_CONE).canWaitHere(), new ScoreStep(IntakeSubsystem.Modes.OUTTAKE)), NodeType.CUBE.atHeight(Height.HIGH), List.of( - new ScoreStep(Elevator.Setpoints.SCORE_HIGH).canWaitHere(), - new ScoreStep(Elevator.Setpoints.SCORE_HIGH, IntakeSubsystem.Modes.OUTTAKE)), + new ScoreStep(Elevator.Setpoints.SCORE_HIGH_CUBE).canWaitHere(), + new ScoreStep(Elevator.Setpoints.SCORE_HIGH_CUBE, IntakeSubsystem.Modes.OUTTAKE)), NodeType.CUBE.atHeight(Height.MID), List.of( - new ScoreStep(Elevator.Setpoints.SCORE_MID).canWaitHere(), - new ScoreStep(Elevator.Setpoints.SCORE_MID, IntakeSubsystem.Modes.OUTTAKE)), + new ScoreStep(Elevator.Setpoints.SCORE_MID_CUBE).canWaitHere(), + new ScoreStep(Elevator.Setpoints.SCORE_MID_CUBE, IntakeSubsystem.Modes.OUTTAKE)), NodeType.CUBE.atHeight(Height.LOW), List.of( - new ScoreStep(Elevator.Setpoints.SCORE_LOW).canWaitHere(), + new ScoreStep(Elevator.Setpoints.SCORE_LOW_CUBE).canWaitHere(), new ScoreStep(IntakeSubsystem.Modes.OUTTAKE))); public static final class Vision { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2445c47..359b55b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -124,7 +124,7 @@ public RobotContainer() { drivebaseSubsystem, translationXSupplier, translationYSupplier, - anthony.rightBumper(), + // anthony.rightBumper(), anthony.leftBumper())); // // FIXME: This error is here to kind of guide you... @@ -304,8 +304,11 @@ private void configureButtonBindings() { .povUp() .onTrue( new ElevatorPositionCommand( - elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE)) - .whileTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, anthony.rightStick())); + elevatorSubsystem, + anthony.rightBumper().getAsBoolean() + ? Constants.Elevator.Setpoints.SHELF_INTAKE_CUBE + : Constants.Elevator.Setpoints.GROUND_INTAKE_CONE)) + .whileTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, anthony.rightBumper())); // reset jacobLayer @@ -323,21 +326,11 @@ private void configureButtonBindings() { anthony .povDown() - .onTrue( - new GroundPickupCommand( - intakeSubsystem, - elevatorSubsystem, - () -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE, - jacob.rightStick())); + .onTrue(new GroundPickupCommand(intakeSubsystem, elevatorSubsystem, jacob.rightStick())); jacob .a() - .onTrue( - new GroundPickupCommand( - intakeSubsystem, - elevatorSubsystem, - () -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE, - jacob.rightStick())); + .onTrue(new GroundPickupCommand(intakeSubsystem, elevatorSubsystem, jacob.rightStick())); jacobLayer .off(jacob.povUp()) @@ -355,7 +348,10 @@ private void configureButtonBindings() { new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, jacob.rightStick()) .alongWith( new ElevatorPositionCommand( - elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE))) + elevatorSubsystem, + jacob.rightStick().getAsBoolean() + ? Constants.Elevator.Setpoints.SHELF_INTAKE_CUBE + : Constants.Elevator.Setpoints.SHELF_INTAKE_CONE))) .onFalse( new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED) .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF))); diff --git a/src/main/java/frc/robot/commands/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/DefaultDriveCommand.java index 8977b85..0ef7a44 100644 --- a/src/main/java/frc/robot/commands/DefaultDriveCommand.java +++ b/src/main/java/frc/robot/commands/DefaultDriveCommand.java @@ -23,21 +23,21 @@ public class DefaultDriveCommand extends CommandBase { private final DoubleSupplier translationYSupplier; private final BooleanSupplier isRobotRelativeForwardSupplier; - private final BooleanSupplier isRobotRelativeBackwardSupplier; + // private final BooleanSupplier isRobotRelativeBackwardSupplier; /** Creates a new DefaultDriveCommand. */ public DefaultDriveCommand( DrivebaseSubsystem drivebaseSubsystem, DoubleSupplier translationXSupplier, DoubleSupplier translationYSupplier, - BooleanSupplier isRobotRelativeForwardSupplier, - BooleanSupplier isRobotRelativeBackwardSupplier) { + BooleanSupplier isRobotRelativeForwardSupplier) { + // BooleanSupplier isRobotRelativeBackwardSupplier) { this.drivebaseSubsystem = drivebaseSubsystem; this.translationXSupplier = translationXSupplier; this.translationYSupplier = translationYSupplier; this.isRobotRelativeForwardSupplier = isRobotRelativeForwardSupplier; - this.isRobotRelativeBackwardSupplier = isRobotRelativeBackwardSupplier; + // this.isRobotRelativeBackwardSupplier = isRobotRelativeBackwardSupplier; addRequirements(drivebaseSubsystem); } @@ -48,12 +48,12 @@ public void execute() { double x = translationXSupplier.getAsDouble(); double y = translationYSupplier.getAsDouble(); Boolean forwardRelative = isRobotRelativeForwardSupplier.getAsBoolean(); - Boolean backwardRelative = isRobotRelativeBackwardSupplier.getAsBoolean(); + // Boolean backwardRelative = isRobotRelativeBackwardSupplier.getAsBoolean(); drivebaseSubsystem.drive( DrivebaseSubsystem.produceChassisSpeeds( forwardRelative, - backwardRelative, + // backwardRelative, x, y, 0, diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java index 1300d0a..0315355 100644 --- a/src/main/java/frc/robot/commands/GroundPickupCommand.java +++ b/src/main/java/frc/robot/commands/GroundPickupCommand.java @@ -11,7 +11,6 @@ import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeSubsystem.Modes; import java.util.function.BooleanSupplier; -import java.util.function.Supplier; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: @@ -25,10 +24,8 @@ public class GroundPickupCommand extends SequentialCommandGroup { public GroundPickupCommand( IntakeSubsystem intakeSubsystem, ElevatorSubsystem elevatorSubsystem, - Supplier modeSupplier, BooleanSupplier isCube) { // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); this.isCube = isCube; this.elevatorSubsystem = elevatorSubsystem; diff --git a/src/main/java/frc/robot/commands/RotateVelocityDriveCommand.java b/src/main/java/frc/robot/commands/RotateVelocityDriveCommand.java index 762972f..5c1234b 100644 --- a/src/main/java/frc/robot/commands/RotateVelocityDriveCommand.java +++ b/src/main/java/frc/robot/commands/RotateVelocityDriveCommand.java @@ -22,7 +22,7 @@ public class RotateVelocityDriveCommand extends CommandBase { private final DoubleSupplier rotationSupplier; private final BooleanSupplier isRobotRelativeForwardSupplier; - private final BooleanSupplier isRobotRelativeBackwardSupplier; + // private final BooleanSupplier isRobotRelativeBackwardSupplier; /** Creates a new RotateVelocityDriveCommand. */ public RotateVelocityDriveCommand( @@ -30,15 +30,15 @@ public RotateVelocityDriveCommand( DoubleSupplier translationXSupplier, DoubleSupplier translationYSupplier, DoubleSupplier rotationSupplier, - BooleanSupplier isRobotRelativeForwardSupplier, - BooleanSupplier isRobotRelativeBackwardSupplier) { + BooleanSupplier isRobotRelativeForwardSupplier) { + // BooleanSupplier isRobotRelativeBackwardSupplier) { this.drivebaseSubsystem = drivebaseSubsystem; this.translationXSupplier = translationXSupplier; this.translationYSupplier = translationYSupplier; this.rotationSupplier = rotationSupplier; this.isRobotRelativeForwardSupplier = isRobotRelativeForwardSupplier; - this.isRobotRelativeBackwardSupplier = isRobotRelativeBackwardSupplier; + // this.isRobotRelativeBackwardSupplier = isRobotRelativeBackwardSupplier; addRequirements(drivebaseSubsystem); } @@ -59,7 +59,7 @@ public void execute() { drivebaseSubsystem.drive( DrivebaseSubsystem.produceChassisSpeeds( isRobotRelativeForwardSupplier.getAsBoolean(), - isRobotRelativeBackwardSupplier.getAsBoolean(), + // isRobotRelativeBackwardSupplier.getAsBoolean(), x, y, rot, diff --git a/src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java index 66f917c..f21e08c 100644 --- a/src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java @@ -557,13 +557,13 @@ public void periodic() { public static ChassisSpeeds produceChassisSpeeds( boolean isRobotRelativeForward, - boolean isRobotRelativeBackward, + // boolean isRobotRelativeBackward, double x, double y, double rotationVelocity, Rotation2d currentGyroAngle) { if (isRobotRelativeForward) return new ChassisSpeeds(x, y, rotationVelocity); - if (isRobotRelativeBackward) return new ChassisSpeeds(-x, -y, rotationVelocity); + // if (isRobotRelativeBackward) return new ChassisSpeeds(-x, -y, rotationVelocity); return ChassisSpeeds.fromFieldRelativeSpeeds(x, y, rotationVelocity, currentGyroAngle); } }