rotations are a massive pain

TitaniumTitans · Jan 18, 2025 · f455d5b · f455d5b
1 parent e26929e
commit f455d5b
Show file tree

Hide file tree

Showing 2 changed files with 38 additions and 5 deletions.
diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java
@@ -7,12 +7,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2025Reefscape";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 70;
-  public static final String GIT_SHA = "206a47d47dab713bee40b1498942da8db41a6a90";
-  public static final String GIT_DATE = "2025-01-17 16:15:49 EST";
+  public static final int GIT_REVISION = 71;
+  public static final String GIT_SHA = "e26929ee1ec4a17a77ea8865a1ef27f7e582b616";
+  public static final String GIT_DATE = "2025-01-17 17:18:26 EST";
   public static final String GIT_BRANCH = "pathplanner";
-  public static final String BUILD_DATE = "2025-01-17 17:17:53 EST";
-  public static final long BUILD_UNIX_TIME = 1737152273609L;
+  public static final String BUILD_DATE = "2025-01-17 20:41:52 EST";
+  public static final long BUILD_UNIX_TIME = 1737164512631L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -4,6 +4,11 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -79,6 +84,34 @@ public Robot() {
   @Override
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
+
+    double xMeters = Units.inchesToMeters(-2.25);
+    double yMeters = Units.inchesToMeters(-2.25);
+    double xOffset = Units.inchesToMeters(10.75);
+    double yOffset = Units.inchesToMeters(9.75);
+    double angle = 0;
+
+    Logger.recordOutput("Blank Pose 3D", new Pose3d());
+    Logger.recordOutput("Blank Pose 3D Array", new Pose3d(), new Pose3d());
+    Logger.recordOutput("90 Intake Pose 3D Array",
+        new Pose3d(
+            new Translation3d(
+                xOffset, //(xOffset * Math.cos(Units.degreesToRadians(angle)) - yOffset * Math.sin(Units.degreesToRadians(angle))),
+                0.3302,
+                yOffset //(yOffset * Math.cos(Units.degreesToRadians(angle)) + xOffset * Math.sin(Units.degreesToRadians(angle)))
+            ),
+            new Rotation3d(0.0, Units.degreesToRadians(angle), Units.degreesToRadians(180.0))
+        ).transformBy(
+            new Transform3d(
+                new Translation3d(
+                    xMeters * Math.cos(Units.degreesToRadians(angle) - yMeters * Math.sin(Units.degreesToRadians(angle))),
+                    0.0,
+                    0.0
+                ),
+                new Rotation3d()
+            )
+        ),
+        new Pose3d());
   }
 
   @Override