trying to get sim to work

FRCTeam3255 · Mar 16, 2024 · d72e4a5 · d72e4a5
1 parent e9d19e0
commit d72e4a5
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 4 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -284,7 +284,9 @@ public static boolean isPracticeBot() {
   public static void updateLoggedPoses() {
     currentRobotPose = subDrivetrain.getPose3d();
     turretPose = subTurret.getAngleAsPose3d();
-    hoodPose = turretPose.plus(subPitch.getAngleAsTransform3d());
+    Pose3d tempPose = subPitch.getAngleAsTransform3d();
+    hoodPose = new Pose3d(turretPose.getTranslation().plus(tempPose.getTranslation()),
+        turretPose.getRotation().rotateBy(tempPose.getRotation()));
   }
 
   // --- PDH ---

diff --git a/src/main/java/frc/robot/subsystems/Pitch.java b/src/main/java/frc/robot/subsystems/Pitch.java
@@ -248,9 +248,10 @@ public Optional<Rotation2d> getDesiredAngleToLock(Pose2d robotPose, Pose3d[] fie
     return Optional.of(desiredLockingAngle);
   }
 
-  public Transform3d getAngleAsTransform3d() {
-    return new Transform3d(new Translation3d(),
-        new Rotation3d(0, -Units.degreesToRadians(desiredPitchAngle), 0));
+  public Pose3d getAngleAsTransform3d() {
+    return new Pose3d(new Translation3d(0, 0, 0), // 0.1, 0, 0.242
+        new Rotation3d(0, Units.rotationsToDegrees(getPitchAngle()),
+            0));
   }
 
   @Override