diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index dab4135..f1ce7d1 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -20,7 +20,6 @@ public class FieldConstants { /** * Units: Meters */ - // public static double FIELD_LENGTH = 12.85875; public static double FIELD_LENGTH = 16.541; /** @@ -33,14 +32,8 @@ public class FieldConstants { * @return An array of field element positions. Your Speaker, Amp, Source, Left * Stage, Center Stage, Right Stage, Subwoofer, Shuffle */ - public static Supplier GET_FIELD_POSITIONS(boolean fairbotics) { + public static Supplier GET_FIELD_POSITIONS() { if (ALLIANCE.isPresent() && ALLIANCE.get().equals(Alliance.Red)) { - if (fairbotics) { - return () -> new Pose3d[] { redConstants.FAIRBOTICS_SPEAKER, redConstants.AMP, redConstants.SOURCE, - redConstants.LEFT_STAGE, - redConstants.CENTER_STAGE, redConstants.RIGHT_STAGE, redConstants.SUBWOOFER, redConstants.SHUFFLE }; - - } return () -> new Pose3d[] { redConstants.SPEAKER_CENTER, redConstants.AMP, redConstants.SOURCE, redConstants.LEFT_STAGE, redConstants.CENTER_STAGE, redConstants.RIGHT_STAGE, redConstants.SUBWOOFER, redConstants.SHUFFLE }; @@ -105,12 +98,6 @@ private static final class redConstants { private static final Pose3d SPEAKER_CENTER = new Pose3d(FIELD_LENGTH - (0.457 / 2), 5.557034, 2.105 - (0.133 / 2), new Rotation3d(0, 0, 0)); - /** - * The coordinate of the center of the red speaker, in meters - */ - private static final Pose3d FAIRBOTICS_SPEAKER = new Pose3d(16.541 - (0.457 / 2), 5.557034, 2.105 - (0.133 / 2), - new Rotation3d(0, 0, 0)); - /** * The coordinate of the center of the red amp, in meters */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ca33cae..fbf02fb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -63,7 +63,6 @@ import frc.robot.commands.ZeroTurret; import frc.robot.commands.autos.AutoInterface; import frc.robot.commands.autos.DefaultAuto; -import frc.robot.commands.autos.Fairbotics1pc; import frc.robot.commands.autos.PreloadOnly; import frc.robot.commands.autos.PreloadTaxi; import frc.robot.commands.autos.WingOnly; @@ -180,7 +179,7 @@ private void configureDriverBindings(SN_XboxController controller) { // Reset Pose controller.btn_South .onTrue(Commands.runOnce( - () -> subDrivetrain.resetPoseToPose(FieldConstants.GET_FIELD_POSITIONS(false).get()[6].toPose2d())) + () -> subDrivetrain.resetPoseToPose(FieldConstants.GET_FIELD_POSITIONS().get()[6].toPose2d())) .alongWith(Commands.runOnce(() -> subDrivetrain.resetDriving(FieldConstants.ALLIANCE)))); controller.btn_LeftTrigger @@ -329,9 +328,6 @@ private void configureNumpadBindings(SN_SwitchboardStick switchboardStick) { } private void configureAutoSelector() { - autoChooser.setDefaultOption("FAIRBOTICS", - new Fairbotics1pc(subShooter, subIntake, subPitch, subTurret, subTransfer, subClimber, subLEDs)); - // PRELOAD ONLY autoChooser.addOption("Preload S1", new PreloadOnly(subDrivetrain, subIntake, subLEDs, subPitch, subShooter, subTransfer, subTurret, subClimber, @@ -368,7 +364,7 @@ private void configureAutoSelector() { false)); // Wing ONLY - autoChooser.addOption("Wing Only Down", + autoChooser.setDefaultOption("Wing Only Down", new WingOnly(subDrivetrain, subIntake, subLEDs, subPitch, subShooter, subTransfer, subTurret, subClimber, true)); diff --git a/src/main/java/frc/robot/commands/AimAuto.java b/src/main/java/frc/robot/commands/AimAuto.java index 045db0f..6d69477 100644 --- a/src/main/java/frc/robot/commands/AimAuto.java +++ b/src/main/java/frc/robot/commands/AimAuto.java @@ -53,7 +53,7 @@ public void initialize() { desiredPitchAngle = subPitch.getPitchAngle(); desiredTurretAngle = Rotation2d.fromDegrees(subTurret.getAngle()); - fieldPoses = FieldConstants.GET_FIELD_POSITIONS(true).get(); + fieldPoses = FieldConstants.GET_FIELD_POSITIONS().get(); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index 19eea5d..d25e48c 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -79,7 +79,7 @@ public Drive( @Override public void initialize() { driveSpeed = (isPracticeBot) ? constDrivetrain.pracBot.DRIVE_SPEED : constDrivetrain.DRIVE_SPEED; - fieldPositions = FieldConstants.GET_FIELD_POSITIONS(false).get(); + fieldPositions = FieldConstants.GET_FIELD_POSITIONS().get(); sourcePose = fieldPositions[2].toPose2d(); leftStage = fieldPositions[3].toPose2d(); diff --git a/src/main/java/frc/robot/commands/LockPitch.java b/src/main/java/frc/robot/commands/LockPitch.java index c05bd1f..b7f708a 100644 --- a/src/main/java/frc/robot/commands/LockPitch.java +++ b/src/main/java/frc/robot/commands/LockPitch.java @@ -47,7 +47,7 @@ public LockPitch(Pitch subPitch, Drivetrain subDrivetrain) { public void initialize() { desiredAngle = subPitch.getPitchAngle(); - fieldPoses = FieldConstants.GET_FIELD_POSITIONS(false).get(); + fieldPoses = FieldConstants.GET_FIELD_POSITIONS().get(); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/LockTurret.java b/src/main/java/frc/robot/commands/LockTurret.java index f23a392..93cf2b3 100644 --- a/src/main/java/frc/robot/commands/LockTurret.java +++ b/src/main/java/frc/robot/commands/LockTurret.java @@ -49,7 +49,7 @@ public void initialize() { @Override public void execute() { // This is in execute in case the alliance changes after command init - fieldPoses = FieldConstants.GET_FIELD_POSITIONS(false).get(); + fieldPoses = FieldConstants.GET_FIELD_POSITIONS().get(); robotPose = subDrivetrain.getPose(); Optional calculatedAngle = subTurret.getDesiredAngleToLock(robotPose, fieldPoses, diff --git a/src/main/java/frc/robot/commands/autos/Fairbotics1pc.java b/src/main/java/frc/robot/commands/autos/Fairbotics1pc.java deleted file mode 100644 index dd44a06..0000000 --- a/src/main/java/frc/robot/commands/autos/Fairbotics1pc.java +++ /dev/null @@ -1,84 +0,0 @@ -// 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. - -package frc.robot.commands.autos; - -import java.util.function.Supplier; - -import com.frcteam3255.joystick.SN_SwitchboardStick; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants.constRobot; -import frc.robot.RobotPreferences.prefIntake; -import frc.robot.RobotPreferences.prefPitch; -import frc.robot.RobotPreferences.prefShooter; -import frc.robot.RobotPreferences.prefTransfer; -import frc.robot.RobotPreferences.prefTurret; -import frc.robot.commands.ShootingPreset; -import frc.robot.commands.TransferGamePiece; -import frc.robot.commands.UnaliveShooter; -import frc.robot.subsystems.Climber; -import frc.robot.subsystems.Intake; -import frc.robot.subsystems.LEDs; -import frc.robot.subsystems.Pitch; -import frc.robot.subsystems.Shooter; -import frc.robot.subsystems.Transfer; -import frc.robot.subsystems.Turret; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class Fairbotics1pc extends SequentialCommandGroup implements AutoInterface { - /** Creates a new Fairbotics1pc. */ - Shooter subShooter; - Intake subIntake; - Pitch subPitch; - Turret subTurret; - Transfer subTransfer; - Climber subClimber; - LEDs subLEDs; - - public Fairbotics1pc(Shooter subShooter, Intake subIntake, Pitch subPitch, Turret subTurret, Transfer subTransfer, - Climber subClimber, LEDs subLEDs) { - this.subShooter = subShooter; - this.subIntake = subIntake; - this.subPitch = subPitch; - this.subTurret = subTurret; - this.subTransfer = subTransfer; - this.subClimber = subClimber; - this.subLEDs = subLEDs; - - addCommands( - // drop the intake - Commands.runOnce(() -> subIntake.setPivotAngle(prefIntake.pivotGroundIntakeAngle.getValue())), - - // move up the pitch - new ShootingPreset(subShooter, subTurret, subPitch, subIntake, - prefShooter.leftShooterSubVelocity.getValue(), - prefShooter.rightShooterSubVelocity.getValue(), - prefTurret.turretSubPresetPos.getValue(), - prefPitch.pitchSubAngle.getValue(), true, new SN_SwitchboardStick(10), "Subwoofer", constRobot.TUNING_MODE) - .withTimeout(4), - - // shoot - Commands.runOnce(() -> subTransfer.setFeederMotorSpeed(prefTransfer.feederShootSpeed.getValue())), - Commands.runOnce(() -> subTransfer.setTransferMotorSpeed(prefTransfer.transferShootSpeed.getValue())), - Commands.waitSeconds(2), - new UnaliveShooter(subShooter, subTurret, subPitch, subLEDs) - - ); - - } - - public Supplier getInitialPose() { - return () -> new Pose2d(); - } - - public Command getAutonomousCommand() { - return this; - } -}