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