diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 3136b1d..52f6933 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/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 aggressively.
+ // 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)
);
diff --git a/src/main/java/frc/robot/subsystems/PhotonVision.java b/src/main/java/frc/robot/subsystems/PhotonVision.java
index 97fe420..974bfcb 100644
--- a/src/main/java/frc/robot/subsystems/PhotonVision.java
+++ b/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
- * a Pose2d
+ * cameras there are on the robot, assuming those cameras see enough tags to generate a pose,
+ * as currently all cameras configuration will generate a Pose2d
*
- * @param consumer The lambda or functional reference that will consume Poses produced
- * by the PhotonVision resource.
+ * @param consumer The lambda, functional reference, or Consumer implementing object
+ * that will consume Poses produced by the PhotonVision resource.
*/
public void addPoseEstimateConsumer(Consumer consumer) {
poseEstimateConsumers.add(consumer);
diff --git a/src/main/java/frc/robot/utilities/Utilities.java b/src/main/java/frc/robot/utilities/Utilities.java
index 77a8651..7c199d1 100644
--- a/src/main/java/frc/robot/utilities/Utilities.java
+++ b/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 = 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