From 4949ba9d035c6b389a9286667d82e755764335cb Mon Sep 17 00:00:00 2001 From: iefomit Date: Wed, 18 Feb 2026 13:08:42 -0800 Subject: [PATCH] Merge main into beta-bot-pull-request - resolved conflicts --- src/main/java/frc/robot/RobotContainer.java | 68 +++++++++++----- .../java/frc/robot/constants/IdConstants.java | 5 +- .../controls/PS5ControllerDriverConfig.java | 79 ++++++++++++------- 3 files changed, 100 insertions(+), 52 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index efcd7c2..ae28fcd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,8 +13,10 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.commands.DoNothing; import frc.robot.commands.drive_comm.DefaultDriveCommand; import frc.robot.commands.vision.ShutdownAllPis; @@ -24,6 +26,7 @@ import frc.robot.constants.VisionConstants; import frc.robot.controls.BaseDriverConfig; import frc.robot.controls.Operator; import frc.robot.controls.PS5ControllerDriverConfig; +import frc.robot.subsystems.Climb.LinearClimb; import frc.robot.subsystems.Intake.Intake; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.drivetrain.GyroIOPigeon2; @@ -56,10 +59,13 @@ public class RobotContainer { private Command auto = new DoNothing(); - // Controllers are defined here private BaseDriverConfig driver = null; private Operator operator = null; + private LinearClimb linearClimb = null; + + // Auto Command selection + private final SendableChooser autoChooser = new SendableChooser<>(); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -94,23 +100,28 @@ public class RobotContainer { // fall-through case Vivace: + linearClimb = new LinearClimb(); case Phil: // AKA "IHOP" case Vertigo: // AKA "French Toast" drive = new Drivetrain(vision, new GyroIOPigeon2()); - driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood, intake, spindexer); + driver = new PS5ControllerDriverConfig(drive, linearClimb); + ((PS5ControllerDriverConfig)driver).setShooter(shooter); + ((PS5ControllerDriverConfig)driver).setTurret(turret); + ((PS5ControllerDriverConfig)driver).setHood(hood); + ((PS5ControllerDriverConfig)driver).setIntake(intake); + ((PS5ControllerDriverConfig)driver).setSpindexer(spindexer); operator = new Operator(drive); // added indexer here for now - // Detected objects need access to the drivetrain DetectedObject.setDrive(drive); - + // SignalLogger.start(); driver.configureControls(); operator.configureControls(); - + initializeAutoBuilder(); registerCommands(); PathGroupLoader.loadPathGroups(); @@ -123,7 +134,7 @@ public class RobotContainer { } drive.setDefaultCommand(new DefaultDriveCommand(drive, driver)); break; - } + } // This is really annoying so it's disabled DriverStation.silenceJoystickConnectionWarning(true); @@ -134,6 +145,7 @@ public class RobotContainer { LiveWindow.setEnabled(false); SmartDashboard.putData("Shutdown Orange Pis", new ShutdownAllPis()); + autoChooserInit(); } /** @@ -144,7 +156,6 @@ public class RobotContainer { drive.setVisionEnabled(enabled); } - public void initializeAutoBuilder() { AutoBuilder.configure( () -> drive.getPose(), @@ -165,6 +176,21 @@ public class RobotContainer { public void registerCommands() { } + /** + * Initialize the SendableChooser on the SmartDashboard. + * Fill the SendableChooser with available Commands. + */ + public void autoChooserInit() { + // add the options to the Chooser + autoChooser.setDefaultOption("Do nothing", new DoNothing()); + autoChooser.addOption("Do nada", new DoNothing()); + autoChooser.addOption("Spin my wheels", new DoNothing()); + autoChooser.addOption("Hello world", new InstantCommand(() -> System.out.println("Hello world"))); + + // put the Chooser on the SmartDashboard + SmartDashboard.putData("Auto chooser", autoChooser); + } + public static BooleanSupplier getAllianceColorBooleanSupplier() { return () -> { // Boolean supplier that controls when the path will be mirrored for the red @@ -181,28 +207,28 @@ public class RobotContainer { } public boolean brownout() { - if(RobotController.getBatteryVoltage() < 6.0) { + if (RobotController.getBatteryVoltage() < 6.0) { return true; - } - else { + } else { return false; } } - public Command getAutoCommand(){ - return auto; + public Command getAutoCommand() { + // get the selected Command + Command autoSelected = autoChooser.getSelected(); + + return autoSelected; } - public void logComponents(){ - if(!Constants.LOG_MECHANISMS) return; - + public void logComponents() { + if (!Constants.LOG_MECHANISMS) + return; + Logger.recordOutput( - "ComponentPoses", - new Pose3d[] { + "ComponentPoses", + new Pose3d[] { // Subsystem Pose3ds - } - ); + }); } } - - diff --git a/src/main/java/frc/robot/constants/IdConstants.java b/src/main/java/frc/robot/constants/IdConstants.java index f171d9a..acaa370 100644 --- a/src/main/java/frc/robot/constants/IdConstants.java +++ b/src/main/java/frc/robot/constants/IdConstants.java @@ -36,11 +36,14 @@ public class IdConstants { // Spindexer public static final int SPINDEXER_ID = 12; + + // Climb + public static final int CLIMB_MOTOR_ID = 8; // Intake // extender right and left motor CAN ID public static final int RIGHT_MOTOR_ID = 1; public static final int LEFT_MOTOR_ID = 2; - // roller motor CAN ID + // roller motor CAN ID public static final int ROLLER_MOTOR_ID = 3; } diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index 75daa51..11f2607 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.Robot; import frc.robot.commands.gpm.AutoShootCommand; import frc.robot.constants.Constants; +import frc.robot.subsystems.Climb.LinearClimb; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.spindexer.Spindexer; @@ -26,55 +27,68 @@ import lib.controllers.PS5Controller.PS5Button; */ public class PS5ControllerDriverConfig extends BaseDriverConfig { private final PS5Controller driver = new PS5Controller(Constants.DRIVER_JOY); - private final BooleanSupplier slowModeSupplier = ()->false; + private final BooleanSupplier slowModeSupplier = () -> false; + private boolean intakeBoolean = true; + private Command autoShoot = null; private Shooter shooter; private Turret turret; private Hood hood; private Intake intake; private Spindexer spindexer; + private LinearClimb climb; - private Command autoShoot; + public PS5ControllerDriverConfig(Drivetrain drive, LinearClimb climb) { + super(drive); + this.climb = climb; + } - private boolean intakeBoolean = true; + public void setIntake(Intake intake) { + this.intake = intake; + } - public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret, Hood hood, Intake intake, Spindexer spindexer) { - super(drive); - this.shooter = shooter; + public void setTurret(Turret turret) { this.turret = turret; + } + + public void setHood(Hood hood) { this.hood = hood; - this.intake = intake; + } + + public void setShooter(Shooter shooter) { + this.shooter = shooter; + } + + public void setSpindexer(Spindexer spindexer) { this.spindexer = spindexer; } - public void configureControls() { + public void configureControls() { // Reset the yaw. Mainly useful for testing/driver practice driver.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw( - new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI) - ))); + new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)))); // Cancel commands - driver.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(()->{ + driver.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> { getDrivetrain().setIsAlign(false); - getDrivetrain().setDesiredPose(()->null); + getDrivetrain().setDesiredPose(() -> null); CommandScheduler.getInstance().cancelAll(); })); // Align wheels driver.get(PS5Button.MUTE).onTrue(new FunctionalCommand( - ()->getDrivetrain().setStateDeadband(false), - getDrivetrain()::alignWheels, - interrupted->getDrivetrain().setStateDeadband(true), - ()->false, getDrivetrain()).withTimeout(2)); + () -> getDrivetrain().setStateDeadband(false), + getDrivetrain()::alignWheels, + interrupted -> getDrivetrain().setStateDeadband(true), + () -> false, getDrivetrain()).withTimeout(2)); // Intake - if(intake != null){ - driver.get(PS5Button.CROSS).onTrue(new InstantCommand(()->{ - if(intakeBoolean){ + if (intake != null) { + driver.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> { + if (intakeBoolean) { intake.extend(); intake.spinStart(); intakeBoolean = false; - } - else{ + } else { intake.retract(); intake.spinStop(); } @@ -82,20 +96,25 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { } // Auto shoot - if(turret != null){ + if (turret != null) { driver.get(PS5Button.SQUARE).onTrue( - new InstantCommand(()->{ - if (autoShoot != null && autoShoot.isScheduled()){ + new InstantCommand(() -> { + if (autoShoot != null && autoShoot.isScheduled()) { autoShoot.cancel(); - } else{ + } else { autoShoot = new AutoShootCommand(turret, getDrivetrain(), hood, shooter, spindexer); CommandScheduler.getInstance().schedule(autoShoot); } - }) - ); + })); } - + if (climb != null) { + driver.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> { + climb.hardstopCalibration(); + })).onFalse(new InstantCommand(() -> { + climb.stopCalibrating(); + })); + } } @Override @@ -133,11 +152,11 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { return false; } - public void startRumble(){ + public void startRumble() { driver.rumbleOn(); } - public void endRumble(){ + public void endRumble() { driver.rumbleOff(); } } -- 2.39.5