fix robot pose for speaker angle math

StuyPulse · Aug 1, 2024 · 82bb153 · 82bb153
1 parent 9d0f125
commit 82bb153
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java
@@ -106,7 +106,7 @@ private double getSpeakerAngle() {
                 new Rotation3d()
             );
 
-            Pose2d robotPose = new Pose2d(Units.inchesToMeters(20.5 * 12 + Units.metersToInches(Settings.LENGTH/2)), speakerPose.getY(), new Rotation2d());
+            Pose2d robotPose = SwerveDrive.getInstance().getPose();
 
             Pose3d armPivotPose = new Pose3d(
                 robotPose.getX() + Settings.DISTANCE_FROM_TOWER_TO_CENTER_OF_ROBOT * robotPose.getRotation().getCos(),