More testing stuff

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

@@ -6,6 +6,8 @@ package frc.robot;
 
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 
+import java.util.OptionalDouble;
+
 import org.littletonrobotics.junction.Logger;
 
 import com.pathplanner.lib.auto.AutoBuilder;
@@ -110,6 +112,9 @@ public class RobotContainer {
             )
         );
 
+        // This needs to be tested after a prolonged amount of driving around <i>aggressively</i>.
+        // Do things like going over the bump repeatedly, spin around a bunch, etc. 
+        // If this works well over time, then this is likely all we need
         driver.a().whileTrue(
             drivetrain.lockRotationToHub(
                 driver::getLeftY, 
@@ -118,6 +123,20 @@ public class RobotContainer {
             ) 
         );
 
+        // This can be tested as an alternative to the above, it's less dynamic, but is a simpler
+        // alternative. 
+        driver.b().whileTrue(
+            drivetrain.lockToYaw(
+                () -> {
+                    OptionalDouble maybeYaw = vision.getBestYawForTag(Utilities.getHubCenterAprilTagID());
+
+                    return maybeYaw.isEmpty() ? 0 : maybeYaw.getAsDouble();
+                }, 
+                driver::getLeftY, 
+                driver::getLeftX
+            )
+        );
+
         // Stop everything by default other than the drivetrain
         shooter.setDefaultCommand(shooter.stop());
         intakePivot.setDefaultCommand(intakePivot.stop());
@@ -163,6 +182,17 @@ public class RobotContainer {
             })
         );
 
+        // While holding D-Pad left on the secondary controller, the hood should move
+        // such that the left trigger on the secondary controller moves the hood down, and
+        // right trigger on the secondary controller moves the hood up.
+        // DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the 
+        // constants file for the subsystem having the problem
+        secondary.povLeft().whileTrue(
+            hood.manualSpeed(() -> {
+                return secondary.getLeftTriggerAxis() * -1 + secondary.getRightTriggerAxis();
+            })
+        );
+
         // STOP STOP STOP STOP STOP STOP STOP STOP STOP STOP STOP STOP STOP STOP STOP STOP
         // Don't proceed unless you've verified by hand or with the above bindings, that sensing
         // systems are providing correct values in the correct direction of rotation. 
@@ -177,6 +207,8 @@ public class RobotContainer {
         // You need to have graphs up of the logged data to make sure the response is correct
         secondary.x().whileTrue(intakePivot.maintainPosition(IntakePivotPosition.kDown));
         secondary.y().whileTrue(intakePivot.maintainPosition(IntakePivotPosition.kUp));
+
+        // TODO Some means of testing hood PIDF
     }
   
     private void configureBindings() {
@@ -197,6 +229,18 @@ public class RobotContainer {
             ) 
         );
 
+        driver.b().whileTrue(
+            drivetrain.lockToYaw(
+                () -> {
+                    OptionalDouble maybeYaw = vision.getBestYawForTag(Utilities.getHubCenterAprilTagID());
+
+                    return maybeYaw.isEmpty() ? 0 : maybeYaw.getAsDouble();
+                }, 
+                driver::getLeftY, 
+                driver::getLeftX
+            )
+        );
+
         shooter.setDefaultCommand(
             shooter.maintainSpeed(ShooterSpeeds.kHubSpeed)
         );

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

@@ -193,11 +193,11 @@ public class PhotonVision extends SubsystemBase {
      * VisualPose. 
      * 
      * The number of Poses produced in a given 20ms cycle is the same number as how many
-     * cameras there are on the robot, as currently all cameras configuration will generate
+     * cameras there are on the robot, assuming those cameras see enough tags to generate a pose, 
-     * a Pose2d
+     * as currently all cameras configuration will generate a Pose2d
      * 
-     * @param consumer The lambda or functional reference that will consume Poses produced
+     * @param consumer The lambda, functional reference, or Consumer implementing object 
-     *   by the PhotonVision resource.
+     *  that will consume Poses produced by the PhotonVision resource.
      */
     public void addPoseEstimateConsumer(Consumer<VisualPose> consumer) {
         poseEstimateConsumers.add(consumer);

src/main/java/frc/robot/utilities/Utilities.java

@@ -51,6 +51,23 @@ public class Utilities {
         }
     }
 
+    /**
+     * Returns the AprilTag ID of the tag that is in the center of the hub
+     * for the robot's assigned alliance. If no alliance is assigned (which is unlikely)
+     * the Blue hub's center tag is returned. 
+     * 
+     * @return The tag ID that is in the center of the assigned alliance's hub
+     */
+    public static int getHubCenterAprilTagID() {
+        Optional<Alliance> alliance = DriverStation.getAlliance();
+
+        if(alliance.isEmpty() || alliance.get() == Alliance.Blue) {
+            return 26;
+        } else {
+            return 10;
+        }
+    }
+
     /**
      * A ChatGPT possible hallucination related to calcuating whether a shot is possible
      * for a given speed and change in X and Y position