// Working

change so that we're finding the arctan using Math.atan instead of rotation2d constructor (constructor does other silly stuff and breaks code)
Patribots4738 · Feb 3, 2024 · 7a723ae · 7a723ae
1 parent aeea48d
commit 7a723ae
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 5 deletions.
diff --git a/src/main/java/frc/robot/commands/ShooterCalc.java b/src/main/java/frc/robot/commands/ShooterCalc.java
@@ -80,7 +80,7 @@ public Command prepareFireCommand(BooleanSupplier shootAtSpeaker, Supplier<Pose2
      */
     public Command prepareFireMovingCommand(BooleanSupplier shootAtSpeaker, Supplier<Pose2d> robotPose) {
         return Commands.run(() -> {
-                SpeedAngleTriplet triplet = calculateSpeed(robotPose.get(), shootAtSpeaker.getAsBoolean());
+                SpeedAngleTriplet triplet = calculateSpeedTesting(robotPose.get(), shootAtSpeaker.getAsBoolean());
 
                 log(triplet);
 
@@ -241,11 +241,11 @@ public Rotation2d calculatePivotAngle(Pose2d robotPose) {
         robotPose = robotPose.relativeTo(FieldConstants.SPEAKER_POSITIONS[positionIndex]);
 
         // Calculate the distance in feet from the robot to the speaker
-        double distanceFeet = robotPose.getTranslation().getNorm();
+        double distanceMeters = robotPose.getTranslation().getNorm();
 
         // Return a new rotation object that represents the pivot angle
         // The pivot angle is calculated based on the speaker's height and the distance to the speaker
-        return new Rotation2d(FieldConstants.SPEAKER_HEIGHT_METERS - NTConstants.PIVOT_OFFSET_Z, distanceFeet);
+        return new Rotation2d(Units.radiansToDegrees(Math.atan2(FieldConstants.SPEAKER_HEIGHT_METERS - NTConstants.PIVOT_OFFSET_Z, distanceMeters)));
     }
 
 

diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java
@@ -438,8 +438,8 @@ public static final class FieldConstants {
         public static final double ALLOWABLE_ERROR_METERS = Units.inchesToMeters(2);
         public static final double FIELD_WIDTH_METERS = 16.5410515;
         public static final double FIELD_HEIGHT_METERS = 8.2112312;
-        public static final double CHAIN_HEIGHT_METERS = 8.2112312;
-        public static final double SPEAKER_HEIGHT_METERS = 2.1082;
+        public static final double CHAIN_HEIGHT_METERS = Units.feetToMeters(4);
+        public static final double SPEAKER_HEIGHT_METERS = 2.082813;
 
         public static Optional<Alliance> ALLIANCE = Optional.empty();