From 62e8fbd5b1d17b12833cd17dbb28ffd55b2fb600 Mon Sep 17 00:00:00 2001
From: TimB-87 <tycheetah109@gmail.com>
Date: Wed, 29 Jan 2025 19:22:56 -0500
Subject: [PATCH] Pull request fixes (robotToCam) and start branchAlign

---
 src/main/java/frc/robot/maps/Shrimp.java      | 10 +++++++---
 src/main/java/frc/robot/subsystems/Drive.java | 12 +++++++++++-
 2 files changed, 18 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/maps/Shrimp.java b/src/main/java/frc/robot/maps/Shrimp.java
index 8b3fcc8..1f37060 100644
--- a/src/main/java/frc/robot/maps/Shrimp.java
+++ b/src/main/java/frc/robot/maps/Shrimp.java
@@ -109,14 +109,18 @@ SDSSwerveModule.MK4_V2.wheelDiameter, new PIDValues(0.004, 0.00,
 
     @Override
     public VisionMap getVisionMap() {
-        // Cam mounted 9.029 sideways of center, 9.029 in. forward of center, 9.75 in.
-        // up from center.
+        // Front left and front right camera locations
+        // Cam mounted 9.029 in. sideways of center(left), 9.029 in. forward of center,
+        // 9.75 in. up from center. Mounted 0 degrees around x axis (roll), angled up
+        // 65.752 degrees, and rotated side-to-side 45 degrees facing left
         Transform3d robotToCamFL = new Transform3d(
                 new Translation3d(Units.inchesToMeters(-9.029), Units.inchesToMeters(9.029),
                         Units.inchesToMeters(9.75)),
                 new Rotation3d(0, Units.degreesToRadians(-65.752), Units.degreesToRadians(45)));
 
-        // Cam mounted facing __, __ forward of center, __ up from center.
+        // Cam mounted 9.029 in. sideways of center(right), 9.029 in. forward of center,
+        // 9.75 in. up from center. Mounted 0 degrees around x axis (roll), angled up
+        // 64.752 degrees, and rotated side-to-side 45 degrees facing right
         Transform3d robotToCamFR = new Transform3d(
                 new Translation3d(Units.inchesToMeters(9.029), Units.inchesToMeters(9.029),
                         Units.inchesToMeters(9.75)),
diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java
index dcb6ef8..93ea6a2 100644
--- a/src/main/java/frc/robot/subsystems/Drive.java
+++ b/src/main/java/frc/robot/subsystems/Drive.java
@@ -29,6 +29,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.FieldConstants.Reef;
 
 public class Drive extends LoggedSubsystem<SwerveDriveData, SwerveDriveMap> {
 
@@ -129,12 +130,21 @@ public Command aimAtReefCenter() {
     // Pull reef coords from FieldConstants somehow
     // Find nearest reef apriltag (do we need a different method of estimation
     // rather than global? Might want to just focus on one tag rather than multiple
-    // behind the robot)
+    // behind the robot. Reef tags are blue 17-22, red 6-11)
     // Use coords and move command to move to either left or right branch
     // (translation to get robotToReef and then move)
     // Need rotation in here somewhere. Logic from rotateTo command, since ideally
     // we move and rotate at the same time
 
+    public enum Branch {
+        leftBranch,
+        rightBranch
+    };
+
+    public Command alignToReefBranch(Branch leftBranch, Branch rightBranch) {
+
+    };
+
     public Command moveInDirection(double xSpeed, double ySpeed, double seconds) {
         return run(() -> {
             move(xSpeed, ySpeed, 0, false);