Adding some more AprilTag stuff, comments for interfaces, and a mess of TODOs

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

@@ -4,11 +4,14 @@
 
 package frc.robot;
 
+import java.util.Optional;
+
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -23,10 +26,19 @@ public class RobotContainer {
 
   private SendableChooser<Command> autoChooser;
 
+  private int ampTagID;
+
+  // TODO There's more than one source tag, how do we want to handle this?
+  private int sourceTagID;
+
+  // TODO There's more than one speaker tag, how do we want to handle this?
+  private int speakerTag;
+
   public RobotContainer() {
     // TODO Need to provide a real initial pose
+    // TODO Need to provide a real IAprilTagProvider, null means we're not using one at all
     // TODO Need to provide a real VisualPoseProvider, null means we're not using one at all
-    drivetrain = new Drivetrain(new Pose2d(), null);
+    drivetrain = new Drivetrain(new Pose2d(), null, null);
 
     // An example Named Command, doesn't need to remain once we start actually adding real things
     // ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration
@@ -38,6 +50,18 @@ public class RobotContainer {
     primary = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
     secondary = new CommandXboxController(OIConstants.kSecondaryXboxUSB);
 
+    Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
+
+    if (alliance.isEmpty() || alliance.get() == DriverStation.Alliance.Red) {
+      ampTagID = 5;
+      sourceTagID = 9;
+      speakerTag = 4; 
+    } else {
+      ampTagID = 6;
+      sourceTagID = 1;
+      speakerTag = 8;
+    }
+
     configureBindings();
   }
 
@@ -66,10 +90,49 @@ public class RobotContainer {
         )
     );
 
-    // This was originally a run while held, not sure that's really necessary
+    // Lock on to the appropriate Amp AprilTag while still being able to strafe and drive forward and back
+    primary.a().onTrue(
+      drivetrain.driveAprilTagLock(
+        primary::getLeftX, 
+        primary::getLeftY, 
+        OIConstants.kTeleopDriveDeadband, 
+        ampTagID, 
+        960 // TODO This is a guess, the vision system should be an ICamera and this should be vision.getWidth()
+      ).until(
+        () -> MathUtil.applyDeadband(primary.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
+      )
+    );
+
+    // Lock on to the appropriate Source AprilTag while still being able to strafe and drive forward and back
+    primary.b().onTrue(
+      drivetrain.driveAprilTagLock(
+        primary::getLeftX, 
+        primary::getLeftY, 
+        OIConstants.kTeleopDriveDeadband, 
+        sourceTagID, 
+        960 // TODO This is a guess, the vision system should be an ICamera and this should be vision.getWidth()
+      ).until(
+        () -> MathUtil.applyDeadband(primary.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
+      )
+    );
+
+    // Lock on to the appropriate Speaker AprilTag while still being able to strafe and drive forward and back
+    primary.x().onTrue(
+      drivetrain.driveAprilTagLock(
+        primary::getLeftX, 
+        primary::getLeftY, 
+        OIConstants.kTeleopDriveDeadband, 
+        speakerTag, 
+        960 // TODO This is a guess, the vision system should be an ICamera and this should be vision.getWidth()
+      ).until(
+        () -> MathUtil.applyDeadband(primary.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
+      )
+    );
+
+    // This was originally a run while held, not sure that's really necessary, change it if need be
     primary.y().onTrue(drivetrain.zeroHeadingCommand());
 
-    // This was originally a run while held, not sure that's really necessary
+    // This was originally a run while held, not sure that's really necessary, change it if need be
     primary.rightBumper().onTrue(drivetrain.setXCommand());
   }
 

src/main/java/frc/robot/constants/DrivetrainConstants.java

@@ -15,8 +15,10 @@ public final class DrivetrainConstants {
     public static final double kRotationalSlewRate = 4.0; // percent per second (1 = 100%)
 
     // Chassis configuration
+    // TODO Update track width
     public static final double kTrackWidth = Units.inchesToMeters(26.5-1.75*2);
     // Distance between centers of right and left wheels on robot
+    // TODO Update wheel base
     public static final double kWheelBase = Units.inchesToMeters(32.3-1.75*2);
     // Distance between front and back wheels on robot
     public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(

src/main/java/frc/robot/interfaces/IAprilTagProvider.java

@@ -2,9 +2,47 @@ package frc.robot.interfaces;
 
 import java.util.OptionalDouble;
 
+/**
+ * An interface which ensures a class can provide common AprilTag oriented
+ * information from various sources in a consistent way.
+ */
 public interface IAprilTagProvider {
+    /**
+     * A method to get the AprilTag offset from the center of the image. This is intended to be used with 
+     * a PIDController, the PIDController keeps the robot rotated such that the AprilTag stays in the horizontal
+     * center of the image. 
+     * 
+     * TODO Is there some other way of ensuring an AprilTag is in the center of the image frame without knowing the horizontal resolution?
+     * TODO Certain providers don't give resolution information natively. Which makes me think there's some other way to do this.
+     * 
+     * @param id The ID of the AprilTag to give an offset from
+     * @param cameraHalfXRes Half of the horizontal resolution of the camera sensor
+     * @return A double with the number of pixels offset from center the tag is, or OptionalDouble.empty() if the tag is not present in the camera's view
+     */
     public OptionalDouble getTagOffsetFromCameraCenterByID(int id, double cameraHalfXRes);
+
+    /**
+     * A method to get the distance from <i>the camera</i> to the AprilTag specified
+     * 
+     * @param id The ID of the AprilTag to give a distance to
+     * @param targetHeightMeters The height of the AprilTag off the ground, in meters
+     * @return The distance, in meters, to the target, or OptionalDouble.empty() if the tag is not present in the camera's view
+     */
     public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters);
+
+    /**
+     * A method to get the pitch of a particular AprilTag
+     * 
+     * @param id The ID of the AprilTag to get the pitch of
+     * @return The pitch, in radians, of the target, or OptionalDouble.empty() if the tag is not present in the camera's view
+     */
     public OptionalDouble getTagPitchByID(int id);
+
+    /**
+     * A method to get the yaw of a particular AprilTag
+     * 
+     * @param id The ID of the AprilTag to get the yaw of
+     * @return The yaw, in radians, of the target, or OptionalDouble.empty() if the tag is not present in the camera's view
+     */
     public OptionalDouble getTagYawByID(int id);
 }

src/main/java/frc/robot/interfaces/ICamera.java

@@ -0,0 +1,21 @@
+package frc.robot.interfaces;
+
+/**
+ * An interface which ensures a class can provide common camera
+ * properties in a consistent way
+ */
+public interface ICamera {
+    /**
+     * Retrieves the width of the camera's resolution
+     * 
+     * @return The width of the resolution, in pixels
+     */
+    public double getWidth();
+
+    /**
+     * Retrieves the height of the camera's resolution
+     * 
+     * @return The height of the resolution, in pixels
+     */
+    public double getHeight();
+}

src/main/java/frc/robot/interfaces/IVisualPoseProvider.java

@@ -4,8 +4,24 @@ import java.util.Optional;
 
 import edu.wpi.first.math.geometry.Pose2d;
 
+/**
+ * An interface which ensures a class' ability to provide visual pose information
+ * in a consistent way
+ */
 public interface IVisualPoseProvider {
+    /**
+     * A record that can contain the two elements necessary for a WPILIB
+     * pose estimator to use the information from a vision system as part of a full
+     * robot pose estimation
+     */
     public record VisualPose(Pose2d visualPose, double timestamp) {}
 
+    /**
+     * Return a VisualPose or null if an empty Optional if none is available.
+     * Implementation should provide an empty response if it's unable to provide
+     * a reliable pose, or any pose at all.
+     * 
+     * @return An Optional containing a VisualPose, or empty if no VisualPose can reliably be provided
+     */
     public Optional<VisualPose> getVisualPose();
 }

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

@@ -13,6 +13,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.util.WPIUtilJNI;
 
 import java.util.Optional;
+import java.util.OptionalDouble;
 import java.util.function.DoubleSupplier;
 
 import com.kauailabs.navx.frc.AHRS;
@@ -24,10 +25,12 @@ import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.utilities.MAXSwerveModule;
 import frc.robot.utilities.SwerveUtils;
+import frc.robot.interfaces.IAprilTagProvider;
 import frc.robot.interfaces.IVisualPoseProvider;
 import frc.robot.interfaces.IVisualPoseProvider.VisualPose;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.PIDCommand;
+import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Drivetrain extends SubsystemBase {
@@ -43,6 +46,8 @@ public class Drivetrain extends SubsystemBase {
     private final SlewRateLimiter m_magLimiter;
     private final SlewRateLimiter m_rotLimiter;
 
+    private final IAprilTagProvider m_aprilTagProvider;
+
     private final IVisualPoseProvider m_visualPoseProvider;
 
     // Odometry class for tracking robot pose
@@ -56,7 +61,7 @@ public class Drivetrain extends SubsystemBase {
     private double m_prevTime;
 
     /** Creates a new DriveSubsystem. */
-    public Drivetrain(Pose2d initialPose, IVisualPoseProvider visualPoseProvider) {
+    public Drivetrain(Pose2d initialPose, IAprilTagProvider aprilTagProvider, IVisualPoseProvider visualPoseProvider) {
         m_frontLeft = new MAXSwerveModule(
             DrivetrainConstants.kFrontLeftDrivingCanId,
             DrivetrainConstants.kFrontLeftTurningCanId,
@@ -80,6 +85,7 @@ public class Drivetrain extends SubsystemBase {
             DrivetrainConstants.kBackRightChassisAngularOffset
         );
 
+        // TODO The NavX is not centered in the robot, it's output probably needs to be translated
         m_gyro = new AHRS(SPI.Port.kMXP);
 
         m_magLimiter = new SlewRateLimiter(DrivetrainConstants.kMagnitudeSlewRate);
@@ -97,6 +103,8 @@ public class Drivetrain extends SubsystemBase {
             initialPose
         );
 
+        m_aprilTagProvider = aprilTagProvider;
+
         m_visualPoseProvider = visualPoseProvider;
 
         m_currentRotation = 0.0;
@@ -307,6 +315,44 @@ public class Drivetrain extends SubsystemBase {
             this);
     }
 
+    public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID, double cameraHorizontalResolution) {
+        if (m_aprilTagProvider == null) {
+            return new PrintCommand("No AprilTag Provider Available");
+        }
+
+        // TODO The process variable is different here than what these constants are used for, may need to use something different
+        PIDController controller = new PIDController(
+            AutoConstants.kPHeadingController, 
+            0, 
+            AutoConstants.kDHeadingController
+        );
+
+        return new PIDCommand(
+            controller, 
+            () -> {
+                OptionalDouble tagOffset = m_aprilTagProvider.getTagOffsetFromCameraCenterByID(tagID, cameraHorizontalResolution);
+
+                if (tagOffset.isEmpty()) {
+                    return 0;
+                }
+
+                return tagOffset.getAsDouble();
+            }, 
+            () -> { return 0; }, 
+            (output) -> {
+                // TODO Field relative or no? What makes sense here, since the intent is to orient to the tag, not the field. 
+                this.drive(
+                    -MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband), 
+                    -MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband), 
+                    output, 
+                    true, 
+                    true
+                );
+            }, 
+            this
+        );
+    }
+
     /**
      * Sets the wheels into an X formation to prevent movement.
      */

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

@@ -19,9 +19,10 @@ import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
 import frc.robot.interfaces.IAprilTagProvider;
+import frc.robot.interfaces.ICamera;
 import frc.robot.interfaces.IVisualPoseProvider;
 
-public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider {
+public class PhotonVision implements ICamera,IAprilTagProvider,IVisualPoseProvider {
 
     private final PhotonCamera camera;
 
@@ -29,8 +30,11 @@ public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider {
 
     private final double cameraHeightMeters;
     private final double cameraPitchRadians;
+    private final double cameraWidth;
+    private final double cameraHeight;
 
-    public PhotonVision(String cameraName, Transform3d robotToCam, double cameraHeightMeters, double cameraPitchRadians) throws IOException {
+    public PhotonVision(String cameraName, Transform3d robotToCam, double cameraHeightMeters, double cameraPitchRadians,
+        double cameraWidth, double cameraHeight) throws IOException {
         camera = new PhotonCamera(cameraName);
 
         photonPoseEstimator = new PhotonPoseEstimator(
@@ -44,6 +48,18 @@ public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider {
 
         this.cameraHeightMeters = cameraHeightMeters;
         this.cameraPitchRadians = cameraPitchRadians;
+        this.cameraWidth = cameraWidth;
+        this.cameraHeight = cameraHeight;
+    }
+
+    @Override
+    public double getWidth() {
+        return cameraWidth;
+    }
+
+    @Override
+    public double getHeight() {
+        return cameraHeight;
     }
 
     @Override