From 37c2fc0a2c0f8214523a59afea1e49dc86bf1402 Mon Sep 17 00:00:00 2001 From: Vignesh Balasubramaniam Date: Wed, 3 Apr 2024 11:11:18 -0500 Subject: [PATCH] Last minute work --- src/main/java/frc/robot/Constants.java | 28 +++++++++---------- src/main/java/frc/robot/RobotContainer.java | 9 ++++-- .../subsystems/drive/DriveSubsystem.java | 2 +- .../robot/subsystems/vision/ObjectCamera.java | 3 +- 4 files changed, 21 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7d97418..e5eb14c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -149,7 +149,7 @@ public static class Shooter { ), false, true, - Units.Degrees.of(0.3).in(Units.Radians), + Units.Degrees.of(0.5).in(Units.Radians), 0.40, 1.04, true @@ -159,20 +159,18 @@ public static class Shooter { Units.DegreesPerSecond.of(360.0 * 10).per(Units.Second) ); public static final List,State>> SHOOTER_MAP = Arrays.asList( - Map.entry(Units.Meters.of(0.00), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(53.0))), - Map.entry(Units.Meters.of(1.50), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(53.0))), - Map.entry(Units.Meters.of(2.00), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(44.0))), - Map.entry(Units.Meters.of(2.20), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(42.0))), - Map.entry(Units.Meters.of(2.50), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(37.0))), - Map.entry(Units.Meters.of(2.80), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(36.5))), - Map.entry(Units.Meters.of(3.00), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(34.0))), - Map.entry(Units.Meters.of(3.20), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(32.5))), - Map.entry(Units.Meters.of(3.50), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(30.5))), - Map.entry(Units.Meters.of(3.75), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(31.0))), - Map.entry(Units.Meters.of(4.00), new State(Units.MetersPerSecond.of(17.0), Units.Degrees.of(28.0))), - Map.entry(Units.Meters.of(4.30), new State(Units.MetersPerSecond.of(17.0), Units.Degrees.of(27.0))), - Map.entry(Units.Meters.of(4.50), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(26.5))), - Map.entry(Units.Meters.of(5.00), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(26.0))) + Map.entry(Units.Meters.of(0.00), new State(Units.MetersPerSecond.of(14.90), Units.Degrees.of(53.0))), + Map.entry(Units.Meters.of(1.00), new State(Units.MetersPerSecond.of(14.94), Units.Degrees.of(53.0))), + Map.entry(Units.Meters.of(1.50), new State(Units.MetersPerSecond.of(15.00), Units.Degrees.of(53.0))), + Map.entry(Units.Meters.of(2.00), new State(Units.MetersPerSecond.of(15.00781), Units.Degrees.of(44.5))), + Map.entry(Units.Meters.of(2.50), new State(Units.MetersPerSecond.of(15.10964), Units.Degrees.of(37.0))), + Map.entry(Units.Meters.of(3.00), new State(Units.MetersPerSecond.of(15.50), Units.Degrees.of(34.0))), + Map.entry(Units.Meters.of(3.50), new State(Units.MetersPerSecond.of(16.25786), Units.Degrees.of(30.0))), + Map.entry(Units.Meters.of(4.00), new State(Units.MetersPerSecond.of(17.00), Units.Degrees.of(27.0))), + Map.entry(Units.Meters.of(4.50), new State(Units.MetersPerSecond.of(17.34737), Units.Degrees.of(25.5))), + Map.entry(Units.Meters.of(5.00), new State(Units.MetersPerSecond.of(17.40), Units.Degrees.of(25.0))), + Map.entry(Units.Meters.of(5.50), new State(Units.MetersPerSecond.of(17.45), Units.Degrees.of(24.0))), + Map.entry(Units.Meters.of(6.00), new State(Units.MetersPerSecond.of(17.50), Units.Degrees.of(23.5))) ); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dbee8bd..989db49 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,6 +6,7 @@ import java.util.function.BooleanSupplier; +import org.apache.commons.math3.ode.SecondaryEquations; import org.lasarobotics.hardware.revrobotics.Blinkin; import com.pathplanner.lib.auto.NamedCommands; @@ -69,6 +70,7 @@ public class RobotContainer { private static final VisionSubsystem VISION_SUBSYSTEM = VisionSubsystem.getInstance(); private static final CommandXboxController PRIMARY_CONTROLLER = new CommandXboxController(Constants.HID.PRIMARY_CONTROLLER_PORT); + private static final CommandXboxController OPERATOR_KEYPAD = new CommandXboxController(Constants.HID.SECONDARY_CONTROLLER_PORT); private static SendableChooser m_automodeChooser = new SendableChooser<>(); @@ -113,7 +115,7 @@ private void configureBindings() { // Right trigger button - aim and shoot at speaker, shooting only if speaker tag is visible and robot is in range // Click DPAD down to override and shoot now - PRIMARY_CONTROLLER.rightTrigger().whileTrue(shootCommand(() -> PRIMARY_CONTROLLER.povDown().getAsBoolean())); + PRIMARY_CONTROLLER.rightTrigger().whileTrue(shootCommand(() -> PRIMARY_CONTROLLER.a().getAsBoolean())); // Right bumper button - amp score, also use for outtake PRIMARY_CONTROLLER.rightBumper().whileTrue(SHOOTER_SUBSYSTEM.scoreAmpCommand()); @@ -154,7 +156,7 @@ private void configureBindings() { ); // B Button - automatically aim at object - PRIMARY_CONTROLLER.b().whileTrue(aimAtObject()); + // PRIMARY_CONTROLLER.b().whileTrue(aimAtObject()); // X button - shoot note into speaker from against the subwoofer PRIMARY_CONTROLLER.x().whileTrue(SHOOTER_SUBSYSTEM.shootSpeakerCommand()); @@ -171,7 +173,8 @@ private void configureBindings() { // DPAD left - PARTY BUTTON!! PRIMARY_CONTROLLER.povLeft().whileTrue(partyMode()); - PRIMARY_CONTROLLER.povDown().onTrue(aimAndIntakeObjectCommand()); + // Operator keypad button 1 - PARTY BUTTON!! + OPERATOR_KEYPAD.button(1).whileTrue(partyMode()); } /** diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 66c043a..a09b008 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -96,7 +96,7 @@ public Hardware(NavX2 navx, public static final Measure> DRIVE_ROTATE_VELOCITY = Units.RadiansPerSecond.of(12 * Math.PI); public static final Measure> AIM_VELOCITY_THRESHOLD = Units.DegreesPerSecond.of(5.0); public static final Measure>> DRIVE_ROTATE_ACCELERATION = Units.RadiansPerSecond.of(4 * Math.PI).per(Units.Second); - public static final Translation2d AIM_OFFSET = new Translation2d(0.0, -0.15); + public static final Translation2d AIM_OFFSET = new Translation2d(0.0, -0.5); public final Measure> DRIVE_MAX_LINEAR_SPEED; public final Measure>> DRIVE_AUTO_ACCELERATION; diff --git a/src/main/java/frc/robot/subsystems/vision/ObjectCamera.java b/src/main/java/frc/robot/subsystems/vision/ObjectCamera.java index bde775c..36d41be 100644 --- a/src/main/java/frc/robot/subsystems/vision/ObjectCamera.java +++ b/src/main/java/frc/robot/subsystems/vision/ObjectCamera.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.vision; -import java.util.ArrayList; import java.util.List; import java.util.Optional; @@ -78,7 +77,7 @@ public PhotonCameraSim getCameraSim() { return m_cameraSim; } - private Optional getBestTarget() { + public Optional getBestTarget() { List targets = m_camera.getLatestResult().getTargets(); PhotonTrackedTarget bestTarget = null;