fix: Remove no-op estimator pose set

chopshop-166 · Jun 27, 2024 · 63a3682 · 63a3682
1 parent c95d01c
commit 63a3682
Show file tree

Hide file tree

Showing 2 changed files with 1 addition and 9 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -129,8 +129,6 @@ public void disabledInit() {
     @Override
     public void configureButtonBindings() {
         driveController.back().onTrue(drive.resetCmd());
-        // Magic numbers for auto testing
-        driveController.start().onTrue(drive.setPoseToEstimate());
         driveController.leftBumper()
                 .whileTrue(drive.robotCentricDrive(() -> {
                     return driveScaler.applyAsDouble(-driveController.getLeftX());

diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java
@@ -9,7 +9,6 @@
 import org.photonvision.PhotonCamera;
 import org.photonvision.PhotonPoseEstimator;
 import org.photonvision.PhotonPoseEstimator.PoseStrategy;
-import org.photonvision.targeting.PhotonTrackedTarget;
 
 import com.chopshop166.chopshoplib.logging.LoggedSubsystem;
 import com.chopshop166.chopshoplib.logging.data.SwerveDriveData;
@@ -53,9 +52,8 @@ public class Drive extends LoggedSubsystem<SwerveDriveData, SwerveDriveMap> {
     double rotationKs = 0.19;
     ProfiledPIDController rotationPID = new ProfiledPIDController(0.065, 0.0, 0.0, new Constraints(240, 270));
     double visionMaxError = 1;
-    Optional<PhotonTrackedTarget> tgt = Optional.empty();
 
-    public SwerveDrivePoseEstimator estimator;
+    SwerveDrivePoseEstimator estimator;
 
     // Vision objects
     private PhotonCamera camera = null;
@@ -118,10 +116,6 @@ public Command setPoseCommand(Supplier<Pose2d> pose) {
         });
     }
 
-    public Command setPoseToEstimate() {
-        return setPoseCommand(() -> estimator.getEstimatedPosition());
-    }
-
     private void deadbandMove(final double xSpeed, final double ySpeed,
             final double rotation, boolean isRobotCentric) {