From 6bfdfadec19fe1591445447ad91468d223f32fbc Mon Sep 17 00:00:00 2001 From: PGgit08 <60079012+PGgit08@users.noreply.github.com> Date: Sat, 27 Jan 2024 19:21:54 -0500 Subject: [PATCH] made speaker poses 3D so less code is required --- src/main/java/frc/robot/Constants.java | 12 +++++------- src/main/java/frc/robot/RobotContainer.java | 4 ---- .../frc/robot/subsystems/SwerveDriveSubsystem.java | 11 ++++------- .../java/frc/robot/subsystems/VisionSubsystem.java | 10 ---------- .../java/frc/robot/utils/AllianceFieldConstants.java | 11 +++-------- 5 files changed, 12 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9bcdf56..4cb95bf 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,13 +4,11 @@ package frc.robot; import com.pathplanner.lib.util.PIDConstants; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import frc.robot.utils.AllianceFieldConstants; -import frc.robot.utils.UtilFuncs; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -133,12 +131,12 @@ public static class Ports { // static field constants public static class FieldConstants { - public static final double SPEAKER_HEIGHT = .200; + public static final double SPEAKER_HEIGHT = 2; public static final int SPEAKER_TAG_BLUE = 7; public static final int SPEAKER_TAG_RED = 4; - public static final Pose2d SPEAKER_POSE_BLUE = new Pose2d(0.25, 5.5, Rotation2d.fromDegrees(180.0)); - public static final Pose2d SPEAKER_POSE_RED = new Pose2d(16.3, 5.5, Rotation2d.fromDegrees(0.0)); + public static final Pose3d SPEAKER_POSE_BLUE = new Pose3d(0.25, 5.5, SPEAKER_HEIGHT, new Rotation3d(0, 0, 180)); + public static final Pose3d SPEAKER_POSE_RED = new Pose3d(16.3, 5.5, SPEAKER_HEIGHT, new Rotation3d(0, 0, 0)); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8b8b38c..909ac21 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,7 +8,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -17,9 +16,7 @@ import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller; -import frc.robot.commands.elevator.HoldElevator; import frc.robot.commands.shooter.AutoAim; -import frc.robot.commands.shooter.HoldShooter; import frc.robot.commands.shooter.SpinShooter; import frc.robot.commands.swerve.BrakeSwerve; import frc.robot.commands.swerve.ResetPose; @@ -29,7 +26,6 @@ import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveDriveSubsystem; import frc.robot.subsystems.VisionSubsystem; -import frc.robot.utils.UtilFuncs; /** * This class is where the bulk of the robot should be declared. Since Command-based is a diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index ddc37a3..ae436a2 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -26,12 +26,10 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -import frc.robot.Constants.FieldConstants; import frc.robot.utils.BNO055; import frc.robot.utils.SwerveModule; import frc.robot.utils.UtilFuncs; @@ -408,17 +406,16 @@ public double[] speakerAngles() { double xSpeakerAngle; double ySpeakerAngle; - int tagID = Constants.FIELD_CONSTANTS.SPEAKER_TAG; - Pose3d tagPose = Constants.FIELD_CONSTANTS.APRILTAG_LAYOUT.getTagPose(tagID).get(); + Pose3d speakerPose = Constants.FIELD_CONSTANTS.SPEAKER_POSE; - Translation2d tagTranslation = new Translation2d(tagPose.getX(), tagPose.getY()); + Translation2d speakerTranslation = new Translation2d(speakerPose.getX(), speakerPose.getY()); Translation2d botTranslation = getPose().getTranslation(); - Translation2d distanceVec = tagTranslation.minus(botTranslation); + Translation2d distanceVec = speakerTranslation.minus(botTranslation); xSpeakerAngle = MathUtil.inputModulus(distanceVec.getAngle().getDegrees(), -180, 180); - double zDifference = FieldConstants.SPEAKER_HEIGHT - Constants.Physical.SHOOTER_HEIGHT_STOWED; // TODO: move to Constants? + double zDifference = speakerPose.getZ() - Constants.Physical.SHOOTER_HEIGHT_STOWED; // TODO: move to Constants? ySpeakerAngle = Math.toDegrees(Math.atan(zDifference / distanceVec.getNorm())); double[] angles = { diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 782776c..5da872c 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -9,7 +9,6 @@ import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; import frc.robot.utils.LimelightHelper; import java.util.Optional; @@ -122,13 +121,4 @@ public double[] tagAngleOffsets(int ID) { return angles; } - - /** - * The tag angle offsets to the tag on the speaker (automatically knows which tag to check). - * - * @return [tx, ty] or null. - */ - public double[] anglesToSpeaker() { - return tagAngleOffsets(Constants.FIELD_CONSTANTS.SPEAKER_TAG); - } } diff --git a/src/main/java/frc/robot/utils/AllianceFieldConstants.java b/src/main/java/frc/robot/utils/AllianceFieldConstants.java index 0666279..865f7fd 100644 --- a/src/main/java/frc/robot/utils/AllianceFieldConstants.java +++ b/src/main/java/frc/robot/utils/AllianceFieldConstants.java @@ -3,7 +3,7 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; -import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.robot.Constants; @@ -15,11 +15,8 @@ public class AllianceFieldConstants { */ public final AprilTagFieldLayout APRILTAG_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - /** The speaker's tag ID. */ - public final int SPEAKER_TAG; - - /** The speaker's 2d pose. */ - public final Pose2d SPEAKER_POSE; + /** The speaker's 3d pose. */ + public final Pose3d SPEAKER_POSE; /** Creates a new AllianceFieldConstants. */ public AllianceFieldConstants() { @@ -28,10 +25,8 @@ public AllianceFieldConstants() { Alliance alliance = UtilFuncs.GetCurrentAlliance(); if (alliance == Alliance.Blue) { - SPEAKER_TAG = Constants.FieldConstants.SPEAKER_TAG_BLUE; SPEAKER_POSE = Constants.FieldConstants.SPEAKER_POSE_BLUE; } else { - SPEAKER_TAG = Constants.FieldConstants.SPEAKER_TAG_RED; SPEAKER_POSE = Constants.FieldConstants.SPEAKER_POSE_RED; } }