add onshape offsets

Author: FarhanJ2

src/main/java/org/steelhawks/Constants.java

@@ -2,6 +2,10 @@
 
 import com.ctre.phoenix6.CANBus;
 import com.pathplanner.lib.path.PathConstraints;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
@@ -280,4 +284,33 @@ public static <T> T loggedValue(String key, T obj) {
         }
         return obj;
     }
+
+    /**
+     * Automatically returns a Transform3d that correctly converts the Onshape coordinate system into the WPILib coordinate system.
+     * <br>
+     * <br>
+     * Learn more <a href="https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html">here</a> about the WPILib coordinate system.
+     * @param x From Onshape
+     * @param y From Onshape
+     * @param z From Onshape
+     * @param roll From Onshape
+     * @param pitch From Onshape
+     * @param yaw From Onshape
+     * @return The WPILib coordinate system ready Transform3d
+     */
+    public static Transform3d fromOnshapeCoordinates(
+        double x, double y, double z,
+        double roll, double pitch, double yaw
+    ) {
+        return new Transform3d(
+            Units.inchesToMeters(y),
+            Units.inchesToMeters(-x),
+            Units.inchesToMeters(-z),
+            new Rotation3d(
+                Units.degreesToRadians(roll),
+                Units.degreesToRadians(pitch),
+                Units.degreesToRadians(yaw  )
+            )
+        );
+    }
 }

src/main/java/org/steelhawks/subsystems/vision/VisionConstants.java

@@ -95,9 +95,9 @@ public static Transform3d[] robotToCamera() {
                         // Up-Down: 6.900835
                         // CHANGE DEPENDING HOW HIGH THE CAMERA IS MOUNTED ON THE BEAM
                         // This assumes that the camera takes up holes 7 and 8 in the beam, counted from the top
-                        Units.inchesToMeters(3.134462),
-                        Units.inchesToMeters(-9.062338),
-                        Units.inchesToMeters(6.900835),
+                        Units.inchesToMeters(3.134462), // y
+                        Units.inchesToMeters(-9.062338), // -x
+                        Units.inchesToMeters(6.900835), // -z
                         new Rotation3d(
                             Units.degreesToRadians(0),
                             Units.degreesToRadians(-15),