From d62e410857bb39ac2cf37f7face321563ceed425 Mon Sep 17 00:00:00 2001 From: Kaya <95276965+kytpbs@users.noreply.github.com> Date: Wed, 19 Jun 2024 18:07:24 +0300 Subject: [PATCH] pull master (#13) * update to use .runEnd instead of run this should fix a lot of the auto issues. * make the green blink a part of the parallel race group * Update path and autos * [Dependency] bump wpilib to 2024.3.2 * rename "SmartShootCommand" to "ShootToSpeakerCommand" This represents what it is about a lot better in my opinion. * Fix tests by not shutting down the hal in teardown * close the subsystem after super in basicIntakeTest * add more comments for explanation * don't shutdown HAL in DriveSubsystem test base --- build.gradle | 2 +- src/main/java/frc/robot/RobotContainer.java | 14 +++++++------- ...hootCommand.java => ShootToSpeakerCommand.java} | 12 ++++++------ .../java/frc/robot/subsystems/DriveSubsystem.java | 13 +++++++------ .../command_tests/arm_tests/BasicIntakeTest.java | 2 +- .../java/command_tests/utils/CommandTestBase.java | 1 - .../DriveSubsystemTestBase.java | 1 - 7 files changed, 22 insertions(+), 23 deletions(-) rename src/main/java/frc/robot/commands/{SmartShootCommand.java => ShootToSpeakerCommand.java} (80%) diff --git a/build.gradle b/build.gradle index 1d18eb8..a8fa62f 100644 --- a/build.gradle +++ b/build.gradle @@ -1,7 +1,7 @@ plugins { id "java" id "jacoco" - id "edu.wpi.first.GradleRIO" version "2024.2.1" + id "edu.wpi.first.GradleRIO" version "2024.3.2" id "com.diffplug.spotless" version "6.23.3" } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9feee06..103b296 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,8 +23,8 @@ import frc.robot.commands.BasicIntakeCommand; import frc.robot.commands.DefaultDriveCommand; import frc.robot.commands.ShootToAmpCommand; +import frc.robot.commands.ShootToSpeakerCommand; import frc.robot.commands.SmartIntakeCommand; -import frc.robot.commands.SmartShootCommand; import frc.robot.commands.led_commands.LEDIdleCommand; import frc.robot.simulationSystems.PhotonSim; import frc.robot.subsystems.ArmSubsystem; @@ -58,7 +58,7 @@ public RobotContainer() { autoChooser = AutoBuilder.buildAutoChooser(); autoChooser.addOption( "Shoot To Shooter", - new SmartShootCommand(shooterSubsystem, intakeSubsystem, armSubsystem, driveSubsystem)); + new ShootToSpeakerCommand(shooterSubsystem, intakeSubsystem, armSubsystem, driveSubsystem)); PhotonCameraSystem.getAprilTagWithID(0); // Load the class before enable. SmartDashboard.putData("Auto Chooser", autoChooser); if (RobotBase.isSimulation()) { @@ -108,7 +108,7 @@ private void setupNamedCommands() { NamedCommands.registerCommand("Intake", new BasicIntakeCommand(intakeSubsystem)); NamedCommands.registerCommand( "Shoot To Speaker", - new SmartShootCommand(shooterSubsystem, intakeSubsystem, armSubsystem, driveSubsystem)); + new ShootToSpeakerCommand(shooterSubsystem, intakeSubsystem, armSubsystem, driveSubsystem)); NamedCommands.registerCommand( "Shoot To Amp", new ShootToAmpCommand(shooterSubsystem, intakeSubsystem, armSubsystem)); } @@ -142,7 +142,7 @@ private void configureJoystickBindings() { new JoystickButton(controller, Button.kY.value) // Shoot, smart (Fully Shoot) .whileTrue( - new SmartShootCommand( + new ShootToSpeakerCommand( shooterSubsystem, intakeSubsystem, armSubsystem, @@ -171,7 +171,7 @@ private void configureJoystickBindings() { .whileTrue(new RunCommand(() -> shooterSubsystem.setShooterSpeed(-1), shooterSubsystem)); new JoystickButton(controller, Button.kLeftBumper.value) - .whileTrue(new SmartShootCommand(shooterSubsystem, intakeSubsystem)); + .whileTrue(new ShootToSpeakerCommand(shooterSubsystem, intakeSubsystem)); new Trigger(() -> controller.getPOV() == 0) // Move arm to 0.5, and set it there until the button is released. @@ -189,10 +189,10 @@ private void configureMidiBindings() { .onTrue(armSubsystem.runOnce(armSubsystem::resetEncoder).ignoringDisable(true)); new JoystickButton(midiController, 3) - .whileTrue(new SmartShootCommand(shooterSubsystem, intakeSubsystem)); + .whileTrue(new ShootToSpeakerCommand(shooterSubsystem, intakeSubsystem)); new JoystickButton(midiController, 4) - .whileTrue(new SmartShootCommand(shooterSubsystem, intakeSubsystem)); + .whileTrue(new ShootToSpeakerCommand(shooterSubsystem, intakeSubsystem)); new JoystickButton(midiController, 16) .onTrue( diff --git a/src/main/java/frc/robot/commands/SmartShootCommand.java b/src/main/java/frc/robot/commands/ShootToSpeakerCommand.java similarity index 80% rename from src/main/java/frc/robot/commands/SmartShootCommand.java rename to src/main/java/frc/robot/commands/ShootToSpeakerCommand.java index c0e1acc..a8ae0ee 100644 --- a/src/main/java/frc/robot/commands/SmartShootCommand.java +++ b/src/main/java/frc/robot/commands/ShootToSpeakerCommand.java @@ -8,10 +8,10 @@ import frc.robot.subsystems.ShooterSubsystem; import java.util.function.DoubleSupplier; -public class SmartShootCommand extends ParallelRaceGroup { +public class ShootToSpeakerCommand extends ParallelRaceGroup { private static final double waitTime = 2.5; - public SmartShootCommand(ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem) { + public ShootToSpeakerCommand(ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem) { super( new SequentialCommandGroup( new BasicRunShooterCommand(shooterSubsystem, waitTime), @@ -20,7 +20,7 @@ public SmartShootCommand(ShooterSubsystem shooterSubsystem, IntakeSubsystem inta new LoadToShooterCommand(intakeSubsystem)))); } - public SmartShootCommand( + public ShootToSpeakerCommand( ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem, ArmSubsystem armSubsystem, @@ -28,10 +28,10 @@ public SmartShootCommand( super( // Constantly move the arm until all the other commands finish. new MoveArmToShooterCommand(armSubsystem, driveSubsystem), - new SmartShootCommand(shooterSubsystem, intakeSubsystem)); + new ShootToSpeakerCommand(shooterSubsystem, intakeSubsystem)); } - public SmartShootCommand( + public ShootToSpeakerCommand( ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem, ArmSubsystem armSubsystem, @@ -43,6 +43,6 @@ public SmartShootCommand( new MoveArmToShooterCommand(armSubsystem, driveSubsystem), new DriveFacingShooter(driveSubsystem, xSpeed, ySpeed), // The finishing command will be this one: - new SmartShootCommand(shooterSubsystem, intakeSubsystem)); + new ShootToSpeakerCommand(shooterSubsystem, intakeSubsystem)); } } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 9d0a8d0..d762b4a 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -220,7 +220,8 @@ private void updatePoseWithVision() { } private Optional getTagPose(int id) { - var tag = PhotonCameraSystem.getFieldLayout().getTagPose(id); + var tagField = PhotonCameraSystem.getFieldLayout(); + var tag = tagField.getTagPose(id); if (tag.isEmpty()) { DriverStation.reportError("Field Layout Couldn't be loaded", false); @@ -251,13 +252,13 @@ public Rotation2d getRotationDifferenceToShooter() { if (tag.isEmpty()) return new Rotation2d(); return tag.get() - .getTranslation() + .getTranslation() // Get the translation of the tag .toTranslation2d() - .minus(getPose().getTranslation()) + .minus(getPose().getTranslation()) // Subtract the robot's translation .getAngle() - .minus(getRotation2d()) - .rotateBy(Rotation2d.fromDegrees(180)) - .times(-1); + .minus(getRotation2d()) // Subtract the robot's rotation + .rotateBy(Rotation2d.fromDegrees(180)) // Rotate by 180 so the back is 0 + .times(-1); // Invert the angle, so the back is a positive angle that can be inverted. } public Pose2d getPose() { diff --git a/src/test/java/command_tests/arm_tests/BasicIntakeTest.java b/src/test/java/command_tests/arm_tests/BasicIntakeTest.java index e73f538..9a0e5e4 100644 --- a/src/test/java/command_tests/arm_tests/BasicIntakeTest.java +++ b/src/test/java/command_tests/arm_tests/BasicIntakeTest.java @@ -35,8 +35,8 @@ public void setUp() { @AfterEach public void tearDown() { - intakeSubsystem.close(); super.tearDown(); + intakeSubsystem.close(); } @Test diff --git a/src/test/java/command_tests/utils/CommandTestBase.java b/src/test/java/command_tests/utils/CommandTestBase.java index ad1c49d..408363d 100644 --- a/src/test/java/command_tests/utils/CommandTestBase.java +++ b/src/test/java/command_tests/utils/CommandTestBase.java @@ -30,6 +30,5 @@ protected void tearDown() { commandScheduler.cancelAll(); commandScheduler.unregisterAllSubsystems(); // ! breaks all test tests if not done commandScheduler.close(); - HAL.shutdown(); } } diff --git a/src/test/java/subsystem_tests/drive_subsystem_tests/DriveSubsystemTestBase.java b/src/test/java/subsystem_tests/drive_subsystem_tests/DriveSubsystemTestBase.java index 8daf74d..4ac0118 100644 --- a/src/test/java/subsystem_tests/drive_subsystem_tests/DriveSubsystemTestBase.java +++ b/src/test/java/subsystem_tests/drive_subsystem_tests/DriveSubsystemTestBase.java @@ -29,6 +29,5 @@ public void tearDown() { commandScheduler.cancelAll(); commandScheduler.unregisterAllSubsystems(); // ! breaks all test tests if not done commandScheduler.close(); - HAL.shutdown(); } }