diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 432b7d15..cf73a344 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,6 +55,10 @@ 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) { @@ -302,6 +310,6 @@ public void enableVision() { } public double distanceToSpeaker() { - return DrivetrainConstants.SPEAKER_POSE.getDistance(getPose().getTranslation()); + return speakerPose.getDistance(getPose().getTranslation()); } }