Merge pull request #62 from Spectrum3847/Vision-Clean-Up

Deleted some duplicate methods in Vision

Author: SpectrumFRC3847

src/main/java/frc/robot/vision/Vision.java

@@ -366,66 +366,7 @@ public void setLimelightPipelines(int pipeline) {
             limelight.setLimelightPipeline(pipeline);
         }
     }
-
-    // ------------------------------------------------------------------------------
-    // Config
-    // ------------------------------------------------------------------------------
-
-    /**
-     * CommandConfig used to create a PIDController and for vision commands using other subsystems
-     * (i.e. swerve)
-     */
-    public static class CommandConfig {
-        public double kp;
-        public double tolerance;
-        public double maxOutput;
-        public double error;
-        public int pipelineIndex;
-        public Limelight limelight;
-        /* For Drive-To commands */
-        public CommandConfig alignCommand;
-        public double verticalSetpoint; // numbers get small as the cone gets closer
-        public double verticalMaxView;
-
-        public void configKp(double kp) {
-            this.kp = kp;
-        }
-
-        public void configTolerance(double tolerance) {
-            this.tolerance = tolerance;
-        }
-
-        public void configMaxOutput(double maxOutput) {
-            this.maxOutput = maxOutput;
-        }
-
-        public void configError(double error) {
-            this.error = error;
-        }
-
-        public void configPipelineIndex(int pipelineIndex) {
-            this.pipelineIndex = pipelineIndex;
-        }
-
-        public void configLimelight(Limelight limelight) {
-            this.limelight = limelight;
-        }
-
-        public void configVerticalSetpoint(double verticalSetpoint) {
-            this.verticalSetpoint = verticalSetpoint;
-        }
-
-        public void configVerticalMaxView(double verticalMaxView) {
-            this.verticalMaxView = verticalMaxView;
-        }
-
-        public void configAlignCommand(CommandConfig alignCommand) {
-            this.alignCommand = alignCommand;
-        }
-
-        public CommandConfig() {}
-    }
-
+    
     // ------------------------------------------------------------------------------
     // Calculation Functions
     // ------------------------------------------------------------------------------
@@ -483,7 +424,7 @@ public double getReefTagAngle() {
     }
 
     public double getAdjustedThetaToReefFace() {
-        int closestReefFace = closestReefFace();
+        int closestReefFace = (int) frontLL.getClosestTagID();
         double[][] reefBlueAngles = {
             {17, 60}, {18, 0}, {19, -60}, {20, -120}, {21, 180}, {22, 120}
         }; // values in theta
@@ -517,33 +458,7 @@ public double getAdjustedThetaToReefFace() {
         return -1; // no reef tag found
     }
 
-    public int closestReefFace() {
-        double[] reefdistance = getDistanceToReefFromRobot();
-        if (reefdistance[0] > -1) {
-            RawFiducial[] tags = frontLL.getRawFiducial();
-            if (Field.isRed()) {
-                int closestReef = tags[0].id;
-                for (RawFiducial tag : tags) {
-                    if (tag.distToRobot < tags[closestReef].distToRobot) {
-                        closestReef = tag.id;
-                    }
-                }
-                Telemetry.print("Closest Reef Face: " + closestReef, PrintPriority.HIGH);
-                return closestReef; // red reef tag
-            } else {
-                int closestReef = tags[0].id;
-                for (RawFiducial tag : tags) {
-                    if (tag.distToRobot < tags[closestReef].distToRobot) {
-                        closestReef = tag.id;
-                    }
-                }
-                Telemetry.print("Closest Reef Face: " + closestReef, PrintPriority.HIGH);
-                return closestReef; // blue reef tag
-            }
-        }
-        Telemetry.print("No reef tag found", PrintPriority.HIGH);
-        return -1; // no reef tag found
-    }
+
 
     /** Returns the distance from the reef in meters, adjusted for the robot's movement. */
     public double[] getDistanceToReefFromRobot() {
@@ -590,7 +505,7 @@ public double[] getDistanceToReefFromRobot() {
      */
     public Translation2d getAdjustedReefPos() {
 
-        int reefID = closestReefFace(); // must call closestReefFace before this method gets passed
+        int reefID = (int) frontLL.getClosestTagID(); // must call closestReefFace before this method gets passed
         Pose2d[] reefFaces = Field.Reef.getCenterFaces();
         double NORM_FUDGE = 0.075;
         // double tunableNoteVelocity = 1;