From b81a6b5be7e7d35b3c01301a1cff927598ae3cb6 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Wed, 11 Feb 2026 16:36:51 -0700 Subject: [PATCH] Remove errrors from merge --- .../java/frc4388/robot/RobotContainer.java | 191 +++++++++++------- src/main/java/frc4388/robot/RobotMap.java | 40 ++-- 2 files changed, 142 insertions(+), 89 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d551676..e422e4f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -23,14 +23,11 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -// Autos -import frc4388.utility.controller.VirtualController; -import frc4388.utility.controller.XboxController; -import frc4388.utility.structs.LEDPatterns; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc4388.robot.commands.MoveForTimeCommand; import frc4388.robot.constants.Constants; -import frc4388.robot.constants.Constants.AutoConstants; -import frc4388.robot.constants.Constants.LEDConstants; import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.constants.Constants.SimConstants.Mode; import frc4388.robot.constants.FieldConstants; @@ -64,10 +61,13 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(Mode.REAL); /* Subsystems */ - public final LED m_robotLED = new LED(LEDConstants.LED_SPARK_ID); + public final LED m_robotLED = new LED(); + //Testing of Colors public final Vision m_vision = new Vision(); - // public final Elevator m_robotElevator = new Elevator(m_robotMap.elevatorIO, m_robotLED); - // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); + public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO); + public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO, m_robotSwerveDrive, m_robotIntake, m_robotLED); + private Boolean operatorManualMode = false; // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); // public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef"); @@ -82,11 +82,11 @@ public class RobotContainer { // public List subsystems = new ArrayList<>(); // ! Teleop Commands - // public void stop() { - // new InstantCommand(()->{}, m_robotSwerveDrive).schedule(); - // m_robotSwerveDrive.stopModules(); - // Constants.AutoConstants.Y_OFFSET_TRIM.set(0); - // } + public void stop() { + new InstantCommand(()->{}, m_robotSwerveDrive).schedule(); + m_robotSwerveDrive.stopModules(); + Constants.AutoConstants.Y_OFFSET_TRIM.set(0); + } // ! /* Autos */ private SendableChooser autoChooser; @@ -113,10 +113,13 @@ public class RobotContainer { configureButtonBindings(); - // DeferredBlock.addBlock(() -> { // Called on first robot enable - // m_robotSwerveDrive.resetGyro(); - // }, false); - DeferredBlock.addBlock(() -> { // Called on every robot enable + // Called on first robot enable + DeferredBlock.addBlock(() -> { + m_robotSwerveDrive.resetGyro(); + }, false); + + // Called on every robot enable + DeferredBlock.addBlock(() -> { TimesNegativeOne.update(); FieldPositions.update(); }, true); @@ -124,27 +127,19 @@ public class RobotContainer { DriverStation.silenceJoystickConnectionWarning(true); - // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + // Drive normally + m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + m_robotSwerveDrive.driveWithInput( + getDeadbandedDriverController().getLeft(), + getDeadbandedDriverController().getRight(),true); - // // IF the driver is holding the aim button, aim the robot towards the hub - // if(m_driverXbox.getRightTriggerAxis() > 0.5) { - // // Aim - // Translation2d shootTarget = new Translation2d(); - // // new Rotation2() - // Rotation2d ang = m_robotSwerveDrive.getPose2d().getTranslation().minus(shootTarget).getAngle(); - // m_robotSwerveDrive.driveFieldAngle( - // getDeadbandedDriverController().getLeft(), - // ang); - // } else { - // // Drive normally - // m_robotSwerveDrive.driveWithInput( - // getDeadbandedDriverController().getLeft(), - // getDeadbandedDriverController().getRight(),true); - // } - - // }, m_robotSwerveDrive) - // .withName("SwerveDrive DefaultCommand")); - // m_robotSwerveDrive.setToSlow(); + }, m_robotSwerveDrive) + .withName("SwerveDrive DefaultCommand")); + + m_robotSwerveDrive.setToSlow(); + + makeAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); } @@ -163,18 +158,9 @@ public class RobotContainer { // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); - - new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.C1C2_GRADIENT))); - - new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.OCEAN_RAINBOW))); - - new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.RAINBOW_RAINBOW))); - - new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotLED.setTo5V())); + + // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + // .onTrue(new RotTo45(m_robotSwerveDrive)); // new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.8 && !operatorManualMode) // .onTrue(RobotShoot) @@ -187,19 +173,86 @@ public class RobotContainer { // new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) // .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive)); - // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); - // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + + + + // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + // .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Active);}, m_robotShooter)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + // .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Inactive);}, m_robotShooter)); + + // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + // .onTrue(new InstantCommand(() -> {;}, m_robotIntake)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + // .onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Retracted);}, m_robotIntake)); + + // new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); + new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotIntake.io.updateGains(); + m_robotShooter.io.updateGains(); + })); - // new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.deactivateLuigiMode())); - } + + // IF the driver is holding the left trigger, intake driving + new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) + .whileTrue(new RunCommand( + () -> { + m_robotSwerveDrive.driveIntake( + getDeadbandedDriverController().getLeft(), + false + ); + }, m_robotSwerveDrive)) + .onTrue(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Extended); + })) + .onFalse(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Retracted); + m_robotSwerveDrive.softStop(); + }, m_robotSwerveDrive)); + + // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready + new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) + .whileTrue(new RunCommand( + () -> { + m_robotSwerveDrive.driveFacingPosition( + getDeadbandedDriverController().getLeft(), + FieldPositions.HUB_POSITION); + }, m_robotSwerveDrive) + ) + .onTrue(new InstantCommand(() -> { + // When Right trigger is pressed, + m_robotIntake.setMode(IntakeMode.Extended); + })) + .onFalse(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Retracted); + m_robotSwerveDrive.softStop(); + }, m_robotSwerveDrive)); + + + // D-PAD fine alignment + new Trigger(() -> getDeadbandedDriverController().getPOV() != -1) + .whileTrue(new RunCommand( + () -> m_robotSwerveDrive.driveFine( + new Translation2d( + 1, + Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV()) + ), + getDeadbandedDriverController().getRight(), 0.15 + ), m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); + } + +//.onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_PINK_HOT))); /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -249,17 +302,17 @@ public class RobotContainer { } autoChooser.onChange((filename) -> { - // autoChooserUpdated = true; - // if (filename.equals("Taxi")) { - // autoCommand = new SequentialCommandGroup( - // new MoveForTimeCommand(m_robotSwerveDrive, - // new Translation2d(0, -1), - // new Translation2d(), 1000, true - // ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)); - // } else { - // autoCommand = new PathPlannerAuto(filename); - // } - // System.out.println("Robot Auto Changed " + filename); + autoChooserUpdated = true; + if (filename.equals("Taxi")) { + autoCommand = new SequentialCommandGroup( + new MoveForTimeCommand(m_robotSwerveDrive, + new Translation2d(0, -1), + new Translation2d(), 1000, true + ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)); + } else { + autoCommand = new PathPlannerAuto(filename); + } + System.out.println("Robot Auto Changed " + filename); }); SmartDashboard.putData(autoChooser); @@ -286,4 +339,4 @@ public class RobotContainer { // public ButtonBox getButtonBox() { // return this.m_buttonBox; // } -} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index e462071..5067e69 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -51,7 +51,7 @@ public class RobotMap { // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); /* Swreve Drive Subsystem */ - // public final SwerveIO swerveDrivetrain; + public final SwerveIO swerveDrivetrain; // /* Shooter and Intake Subsystem */ public final ShooterIO shooterIO; @@ -75,13 +75,13 @@ public class RobotMap { // reverseLidar = new LidarReal(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL); // Configure swerve drive train - // SwerveDrivetrain swerveDrivetrainReal = new SwerveDrivetrain (TalonFX::new, TalonFX::new, CANcoder::new, - // SwerveDriveConstants.DrivetrainConstants, - // SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT, - // SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT - // ); + SwerveDrivetrain swerveDrivetrainReal = new SwerveDrivetrain (TalonFX::new, TalonFX::new, CANcoder::new, + SwerveDriveConstants.DrivetrainConstants, + SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT, + SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT + ); - // swerveDrivetrain = new SwerveReal(swerveDrivetrainReal); + swerveDrivetrain = new SwerveReal(swerveDrivetrainReal); // Configure Shooter 22,23,24 TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id, Constants.CANIVORE_CANBUS); @@ -103,7 +103,7 @@ public class RobotMap { intakeIO = new IntakeReal(arm, roller); // Fault - // FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); + FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); FaultTalonFX.addDevice(shooter1, "Shooter1"); @@ -112,18 +112,18 @@ public class RobotMap { FaultTalonFX.addDevice(arm, "Arm"); FaultTalonFX.addDevice(roller, "Roller"); - // FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive"); - // FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer"); - // FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(0).getEncoder(), "Module 0 CANCoder"); - // FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getDriveMotor(), "Module 1 Drive"); - // FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getSteerMotor(), "Module 1 Steer"); - // FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(1).getEncoder(), "Module 1 CANCoder"); - // FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getDriveMotor(), "Module 2 Drive"); - // FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getSteerMotor(), "Module 2 Steer"); - // FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(2).getEncoder(), "Module 2 CANCoder"); - // FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getDriveMotor(), "Module 3 Drive"); - // FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getSteerMotor(), "Module 3 Steer"); - // FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(0).getEncoder(), "Module 0 CANCoder"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getDriveMotor(), "Module 1 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getSteerMotor(), "Module 1 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(1).getEncoder(), "Module 1 CANCoder"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getDriveMotor(), "Module 2 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getSteerMotor(), "Module 2 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(2).getEncoder(), "Module 2 CANCoder"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getDriveMotor(), "Module 3 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getSteerMotor(), "Module 3 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder"); break; // case SIM: