Last minute work

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<Entry<Measure<Distance>,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)))
     );
   }
 

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<Command> 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());
   }
 
   /**

src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java

@@ -96,7 +96,7 @@ public Hardware(NavX2 navx,
   public static final Measure<Velocity<Angle>> DRIVE_ROTATE_VELOCITY = Units.RadiansPerSecond.of(12 * Math.PI);
   public static final Measure<Velocity<Angle>> AIM_VELOCITY_THRESHOLD = Units.DegreesPerSecond.of(5.0);
   public static final Measure<Velocity<Velocity<Angle>>> 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<Velocity<Distance>> DRIVE_MAX_LINEAR_SPEED;
   public final Measure<Velocity<Velocity<Distance>>> DRIVE_AUTO_ACCELERATION;
 

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<PhotonTrackedTarget> getBestTarget() {
+  public Optional<PhotonTrackedTarget> getBestTarget() {
     List<PhotonTrackedTarget> targets = m_camera.getLatestResult().getTargets();
 
     PhotonTrackedTarget bestTarget = null;