deadband to swerve/elevator movement, note align, and auto aim run re…

…peatedly

src/main/java/frc/robot/RobotContainer.java

@@ -160,8 +160,8 @@ private void configureBindings() {
       );
     }, _swerveSubsystem));
 
-    _driveController.L2().onTrue(
-      new AutoAim( // TODO: test
+    _driveController.L2().whileTrue(
+      new AutoAim(
         _swerveSubsystem,
         _shooterSubsystem, 
         _elevatorSubsystem,
@@ -177,7 +177,9 @@ private void configureBindings() {
         _swerveSubsystem,
         _shooterSubsystem,
         _elevatorSubsystem,
-        _ledSubsystem
+        _ledSubsystem,
+        () -> MathUtil.applyDeadband(-_driveFilterLeftY.calculate(_driveController.getLeftY()), 0.05),
+        () -> MathUtil.applyDeadband(-_driveFilterLeftX.calculate(_driveController.getLeftX()), 0.05)
       )
     );
   }

src/main/java/frc/robot/commands/auto/AutoAim.java

@@ -76,7 +76,7 @@ public AutoAim(
     DoubleSupplier xSpeed,
     DoubleSupplier ySpeed
   ) {
-    this(swerve, shooter, elevator, leds, xSpeed, ySpeed, swerve::speakerHeading, shooter::speakerAngle, elevator::speakerHeight, true);
+    this(swerve, shooter, elevator, leds, xSpeed, ySpeed, swerve::speakerHeading, shooter::speakerAngle, elevator::speakerHeight, false);
   }
 
   /** 

src/main/java/frc/robot/subsystems/ElevatorSubsystem.java

@@ -121,6 +121,8 @@ public void driveElevator(double speed) {
 
     ff = UtilFuncs.FromVolts(ff);
 
+    speed = MathUtil.applyDeadband(speed, 0.08);
+
     _leftMotor.set(ff + speed);
   }
 

src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java

@@ -253,6 +253,10 @@ public double getDriveCoeff() {
    * @see ChassisSpeeds (wpilib chassis speeds class)
    */
   public void driveChassis(ChassisSpeeds chassisSpeeds) {
+    chassisSpeeds.vxMetersPerSecond = MathUtil.applyDeadband(chassisSpeeds.vxMetersPerSecond, .01);
+    chassisSpeeds.vyMetersPerSecond = MathUtil.applyDeadband(chassisSpeeds.vyMetersPerSecond, .01);
+    chassisSpeeds.omegaRadiansPerSecond = MathUtil.applyDeadband(chassisSpeeds.omegaRadiansPerSecond, Math.PI / 20);
+
     // IMPORTANT: X-axis and Y-axis are flipped (based on wpilib coord system)
     if (fieldOriented) {
       double relativeHeading = getHeading().getDegrees() + (UtilFuncs.GetAlliance() == Alliance.Red ? 180 : 0);

src/main/java/frc/robot/subsystems/VisionSubsystem.java

@@ -1,8 +1,12 @@
 /* Copyright (C) 2024 Team 334. All Rights Reserved.*/
 package frc.robot.subsystems;
 
+import java.lang.reflect.Array;
+import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.Collections;
 import java.util.Comparator;
+import java.util.List;
 import java.util.Optional;
 
 import javax.swing.text.html.Option;
@@ -19,6 +23,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.FieldConstants;
+import frc.robot.utils.NoteSort;
 import frc.robot.utils.UtilFuncs;
 import frc.robot.utils.helpers.LimelightHelper;
 
@@ -185,31 +190,8 @@ public double[] tagAngleOffsets(int ID) {
   }
 
   public double[] getNoteAngles() {
-    ArrayNode targets = _intake.getNeuralTargets();
-
-    // Collections.sort(
-    //   targets, new Comparator() {
-    //     @Override
-    //     public int compare(JsonNode noteA, JsonNode noteB) {
-    //       double noteA_area = noteA.get("ta").asDouble();
-    //       double noteB_area = noteB.get("ta").asDouble();
-
-    //       if (noteA_area > noteB_area) {
-    //         return 1;
-    //       }
-
-    //       if (noteB_area > noteA_area) {
-    //         return -1;
-    //       }
-
-    //       else {
-    //         return 0;
-    //       }
-    //     }
-    //   }
-    // );
-
-    // return {0, 0};
+    ArrayList<JsonNode> targets = _intake.getNeuralTargets();
+    Arrays.sort(targets.toArray(new JsonNode[targets.size()]), new NoteSort());
 
     JsonNode target = targets.get(0);
 

src/main/java/frc/robot/utils/NoteSort.java

@@ -0,0 +1,20 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.utils;
+
+import java.util.Comparator;
+
+import com.fasterxml.jackson.databind.JsonNode;
+
+public class NoteSort implements Comparator<JsonNode> {
+    @Override
+    public int compare(JsonNode noteA, JsonNode noteB)
+    {
+        double ta_A = noteA.get("ta").asDouble();
+        double ta_B = noteB.get("ta").asDouble();
+
+        return (int) Math.signum(ta_A - ta_B);
+    }
+}

src/main/java/frc/robot/utils/helpers/LimelightHelper.java

@@ -1,6 +1,8 @@
 /* Copyright (C) 2024 Team 334. All Rights Reserved.*/
 package frc.robot.utils.helpers;
 
+import java.util.ArrayList;
+
 import com.fasterxml.jackson.databind.JsonNode;
 import com.fasterxml.jackson.databind.ObjectMapper;
 import com.fasterxml.jackson.databind.node.ArrayNode;
@@ -44,10 +46,15 @@ public JsonNode getJson() {
    * Returns the neural targets from the limelight.
    * 
    */
-  public ArrayNode getNeuralTargets() {
+  public ArrayList<JsonNode> getNeuralTargets() {
     ArrayNode targets = (ArrayNode) getJson().get("Results").get("Detector");
+    ArrayList<JsonNode> out = new ArrayList<>();
+
+    for (JsonNode t : targets) {
+      out.add(t);
+    }
 
-    return targets;
+    return out;
   }
 
   /**