From e8e4aa27347a6761559a6e892ac3f26830cc7bbb Mon Sep 17 00:00:00 2001 From: Nate Bailey Date: Sat, 9 Mar 2024 09:45:24 -0500 Subject: [PATCH 1/3] [#400] fixed distanceToTarget() --- src/main/java/frc/robot/subsystems/Swerve.java | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 432b7d15..9d851f44 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -1,5 +1,6 @@ package frc.robot.subsystems; +import java.sql.Driver; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -13,6 +14,7 @@ import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; @@ -23,6 +25,7 @@ import frc.robot.Constants.CollisionConstants; import frc.robot.Constants.DrivetrainConstants; import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.VisionConstants; import frc.thunder.filter.XboxControllerFilter; import frc.thunder.shuffleboard.LightningShuffleboard; import frc.thunder.util.Pose4d; @@ -43,6 +46,7 @@ public class Swerve extends SwerveDrivetrain implements Subsystem { private boolean robotCentricControl = false; private double maxSpeed = DrivetrainConstants.MaxSpeed; private double maxAngularRate = DrivetrainConstants.MaxAngularRate * DrivetrainConstants.ROT_MULT; + private Translation2d speakerPose = VisionConstants.BLUE_SPEAKER_LOCATION.toTranslation2d(); public Swerve(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, SwerveModuleConstants... modules) { @@ -51,8 +55,12 @@ public Swerve(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpda initLogging(); configurePathPlanner(); - } + if (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { + speakerPose = VisionConstants.RED_SPEAKER_LOCATION.toTranslation2d(); + } + } + public void applyVisionPose(Pose4d pose) { if (!disableVision) { addVisionMeasurement(pose.toPose2d(), pose.getFPGATimestamp(), pose.getStdDevs()); @@ -302,6 +310,6 @@ public void enableVision() { } public double distanceToSpeaker() { - return DrivetrainConstants.SPEAKER_POSE.getDistance(getPose().getTranslation()); + return speakerPose.getDistance(getPose().getTranslation()); } } From 5a0ed29c6cca7a05e7fa171ac03a78d60d4b0085 Mon Sep 17 00:00:00 2001 From: Nate Bailey Date: Sat, 9 Mar 2024 09:53:48 -0500 Subject: [PATCH 2/3] [#400] fine matthew. --- src/main/java/frc/robot/subsystems/Swerve.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 9d851f44..8f81bc7d 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -56,11 +56,11 @@ public Swerve(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpda configurePathPlanner(); - if (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { + if (DriverStation.getAlliance().get() != null && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { speakerPose = VisionConstants.RED_SPEAKER_LOCATION.toTranslation2d(); } } - + public void applyVisionPose(Pose4d pose) { if (!disableVision) { addVisionMeasurement(pose.toPose2d(), pose.getFPGATimestamp(), pose.getStdDevs()); From 72bcde96a4f594fb6a2f897ca7a05e6cbabe03ea Mon Sep 17 00:00:00 2001 From: Nate Bailey Date: Sat, 9 Mar 2024 09:58:08 -0500 Subject: [PATCH 3/3] [#400] nevermind --- src/main/java/frc/robot/subsystems/Swerve.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 8f81bc7d..cf73a344 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -56,7 +56,7 @@ public Swerve(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpda configurePathPlanner(); - if (DriverStation.getAlliance().get() != null && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { + if (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { speakerPose = VisionConstants.RED_SPEAKER_LOCATION.toTranslation2d(); } }