From 7a723ae7ba6ae1610a3b745cf20a96b99927ce7e Mon Sep 17 00:00:00 2001 From: RudyG252 <122180943+RudyG252@users.noreply.github.com> Date: Sat, 3 Feb 2024 15:32:22 -0800 Subject: [PATCH] // Working change so that we're finding the arctan using Math.atan instead of rotation2d constructor (constructor does other silly stuff and breaks code) --- src/main/java/frc/robot/commands/ShooterCalc.java | 6 +++--- src/main/java/frc/robot/util/Constants.java | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/commands/ShooterCalc.java b/src/main/java/frc/robot/commands/ShooterCalc.java index 590600be..83ecf421 100644 --- 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 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 index 831b6024..00bd37dc 100644 --- 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 = Optional.empty();