Merge branch 'main' of https://git.coldlightalchemist.com/Team_2648/2025_Robot_Code

src/main/deploy/pathplanner/autos/One Coral Left.auto

@@ -0,0 +1,25 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Start to 60 Right"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Two Coral Left.auto

@@ -0,0 +1,49 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Start to 60 Right"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "60 Right to HP"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Collect Coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HP to 300 Left"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/300 Right to HP.path

@@ -37,12 +37,6 @@
   "constraintZones": [],
   "pointTowardsZones": [],
   "eventMarkers": [
-    {
-      "name": "Score L4",
-      "waypointRelativePos": 0,
-      "endWaypointRelativePos": null,
-      "command": null
-    },
     {
       "name": "HP Pickup",
       "waypointRelativePos": 0.16666666666666663,

src/main/deploy/pathplanner/paths/60 Right to HP.path

@@ -0,0 +1,61 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.988527397260274,
+        "y": 5.257106164383561
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.649657534252619,
+        "y": 6.474186643842743
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.3072345890410957,
+        "y": 7.135316780821918
+      },
+      "prevControl": {
+        "x": 2.1636986301369863,
+        "y": 6.188698630136986
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "HP Left Position"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "HP Pickup",
+      "waypointRelativePos": 0.20476190476190542,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -53.97262661489646
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -120.06858282186238
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/HP to 300 Left.path

@@ -12,16 +12,16 @@
         "y": 6.909931506849315
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "HP Left Position"
     },
     {
       "anchor": {
-        "x": 4.026883561643835,
+        "x": 3.973766447368421,
-        "y": 5.257106164383561
+        "y": 5.246957236842105
       },
       "prevControl": {
-        "x": 3.7113441780821916,
+        "x": 3.6582270638067773,
-        "y": 5.783005136986301
+        "y": 5.772856209444845
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [
     {
-      "name": "Score L4",
+      "name": "Lift L4",
-      "waypointRelativePos": 0,
+      "waypointRelativePos": 0.5,
       "endWaypointRelativePos": null,
       "command": null
     }

src/main/deploy/pathplanner/paths/Start to 60 Right.path

@@ -0,0 +1,61 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.587970890410959,
+        "y": 6.143621575342466
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.385916095890411,
+        "y": 6.158647260273973
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.988527397260274,
+        "y": 5.227054794520548
+      },
+      "prevControl": {
+        "x": 5.574529109589041,
+        "y": 6.098544520547945
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0.4547619047619047,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -121.60750224624898
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,31 @@
+package frc.robot.constants;
+
+public class VisionConstants {
+    // global coordinate map of all tags. index is the tag id.
+    // Units: inches and degrees. {x, y, z, z-rotation, y-rotation}
+    // This is for ANDYMARK FIELDS found in NE. Not for WELDED FIELDS.
+    public static final double[][] globalTagCoords = {{},
+        {656.98, 24.73, 58.50, 126.0, 0},
+        {656.98, 291.90, 58.50, 234.0, 0},
+        {452.4, 316.21, 51.25, 270, 0},
+        {365.2, 241.44, 73.54, 0, 30},
+        {365.2, 75.19, 73.54, 0, 30},
+        {530.49, 129.97, 12.13, 300, 0},
+        {546.87, 158.3, 12.13, 0, 0},
+        {530.49, 186.63, 12.13, 60, 0},
+        {497.77, 186.63, 12.13, 120, 0},
+        {481.39, 158.3, 12.13, 180, 0},
+        {497.77, 129.97, 12.13, 240, 0},
+        {33.9, 24.73, 58.5, 54, 0},
+        {33.9, 291.9, 58.5, 306, 0},
+        {325.68, 241.44, 73.54, 180, 30},
+        {325.68, 75.19, 73.54, 180, 30},
+        {238.49, 0.42, 51.25, 90, 0},
+        {160.39, 129.97, 12.13, 240, 0},
+        {144.00, 158.3, 12.13, 180, 0},
+        {160.39, 186.63, 12.13, 120, 0},
+        {193.1, 186.63, 12.13, 60, 0},
+        {209.49, 158.3, 12.13, 0, 0},
+        {193.1, 129.97, 12.13, 300, 0},
+    };
+}

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

@@ -1,53 +1,103 @@
 package frc.robot.subsystems;
 
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.networktables.BooleanSubscriber;
 import edu.wpi.first.networktables.DoubleSubscriber;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import frc.robot.constants.VisionConstants;
 
 public class Vision{
 
-    private DoubleSubscriber blackRobotRelativePose;
+    private DoubleSubscriber blackRobotRelativeX;
+    private DoubleSubscriber blackRobotRelativeY;
+    private DoubleSubscriber blackRobotRelativeZ;
+
     private DoubleSubscriber blackClosestTag;
     private BooleanSubscriber blackTagDetected;
 
     private DoubleSubscriber blackFramerate;
 
-    private DoubleSubscriber orangeRobotRelativePose;
+    private DoubleSubscriber orangeRobotRelativeX;
+    private DoubleSubscriber orangeRobotRelativeY;
+    private DoubleSubscriber orangeRobotRelativeZ;
+
     private DoubleSubscriber orangeClosestTag;
     private BooleanSubscriber orangeTagDetected;
 
     private DoubleSubscriber orangeFramerate;
 
-    public Vision(){
+    private DoubleSupplier gyroAngle;
+
+    public Vision(DoubleSupplier gyroAngle){
         NetworkTableInstance inst = NetworkTableInstance.getDefault();
 
         NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
         NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial");
 
-        blackRobotRelativePose = blackVisionTable.getDoubleTopic("blackRelativePose").subscribe(0.0);
+        blackRobotRelativeX = orangeVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0);
+        blackRobotRelativeY = orangeVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0);
+        blackRobotRelativeZ = orangeVisionTable.getDoubleTopic("blackRelativeZ").subscribe(0.0);
+
         blackClosestTag = blackVisionTable.getDoubleTopic("blackClosestTag").subscribe(0.0);
         blackTagDetected = blackVisionTable.getBooleanTopic("blackTagDetected").subscribe(false);
 
         blackFramerate = blackVisionTable.getDoubleTopic("blackFPS").subscribe(0.0);
 
-        orangeRobotRelativePose = orangeVisionTable.getDoubleTopic("orangeRelativePose").subscribe(0.0);
+        orangeRobotRelativeX = orangeVisionTable.getDoubleTopic("orangeRelativeX").subscribe(0.0);
+        orangeRobotRelativeY = orangeVisionTable.getDoubleTopic("orangeRelativeY").subscribe(0.0);
+        orangeRobotRelativeZ = orangeVisionTable.getDoubleTopic("orangeRelativeZ").subscribe(0.0);
+
         orangeClosestTag = orangeVisionTable.getDoubleTopic("orangeClosestTag").subscribe(0.0);
         orangeTagDetected = orangeVisionTable.getBooleanTopic("orangeTagDetected").subscribe(false);
 
         orangeFramerate = orangeVisionTable.getDoubleTopic("orangeFPS").subscribe(0.0);
     }
     
-    public double getBlackGlobalPose(){
+    public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords, Rotation2d gyroAngle){
-        return blackRobotRelativePose.get();
+        Pose2d tag2dPose = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
+            VisionConstants.globalTagCoords[tagID][1],
+            new Rotation2d());
+
+        Pose2d relative = new Pose2d(relativeCoords, gyroAngle);
+
+        Transform2d relative2dTransformation = new Transform2d(relative.getTranslation(), relative.getRotation());
+
+        Pose2d globalPose = tag2dPose.transformBy(relative2dTransformation.inverse());
+
+        return new Pose2d(globalPose.getTranslation(), gyroAngle);
     }
 
-    public double getBlackClosestTag(){
+    public Pose2d getBlackGlobalPose(){
-        return blackClosestTag.get();
+        
+        return relativeToGlobalPose2d(getBlackClosestTag(), 
+            new Translation2d(getBlackRelativeX(), getBlackRelativeY()), 
+            new Rotation2d(gyroAngle.getAsDouble()));
+    }
+
+    public double getBlackRelativeX(){
+        return blackRobotRelativeX.get();
+    }
+
+    public double getBlackRelativeY(){
+        return blackRobotRelativeY.get();
+    }
+
+    public double getBlackRelativeZ(){
+        return blackRobotRelativeZ.get();
+    }
+
+    public int getBlackClosestTag(){
+        return (int) blackClosestTag.get();
     }
 
     public double getBlackTimeStamp(){
-        return blackRobotRelativePose.getLastChange();
+        return blackRobotRelativeX.getLastChange();
     }
 
     public boolean getBlackTagDetected(){
@@ -58,16 +108,31 @@ public class Vision{
         return blackFramerate.get();
     }
 
-    public double getOrangeGlobalPose(){
+    public Pose2d getOrangeGlobalPose(){
-        return orangeRobotRelativePose.get();
+        
+        return relativeToGlobalPose2d(getBlackClosestTag(), 
+            new Translation2d(getBlackRelativeX(), getBlackRelativeY()), 
+            new Rotation2d(gyroAngle.getAsDouble()));
     }
     
-    public double getOrangeClosestTag(){
+    public double getOrangeRelativeX(){
-        return orangeClosestTag.get();
+        return orangeRobotRelativeX.get();
+    }
+
+    public double getOrangeRelativeY(){
+        return orangeRobotRelativeY.get();
+    }
+
+    public double getOrangeRelativeZ(){
+        return orangeRobotRelativeZ.get();
+    }
+
+    public int getOrangeClosestTag(){
+        return (int) orangeClosestTag.get();
     }
 
     public double getOrangeTimeStamp(){
-        return orangeRobotRelativePose.getLastChange();
+        return orangeRobotRelativeX.getLastChange();
     }
 
     public boolean getOrangeTagDetected(){