From 2de45da486a26cc2b88735d7a685b96459be95eb Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Fri, 19 Mar 2021 13:43:45 -0600
Subject: [PATCH] Making Galactic path work

---
 src/main/java/frc4388/robot/Constants.java    | 11 +++----
 .../robot/commands/auto/IdentifyPath.java     | 30 +++++++++++++++----
 .../robot/commands/shooter/TrackTarget.java   |  1 +
 3 files changed, 31 insertions(+), 11 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 16da8d0..2d1c58c 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -217,11 +217,12 @@ public final class Constants {
 
             //Galactic Search
         public static final double searchError = 0.5;
-        public static final double bothCloseVisibleY = -16.7;
-        public static final double closeLeftVisibleY = -11.47;
-        public static final double closeRightVisibleY = -10.50;
-        public static final double farLeftVisibleX = -1.03;
-        public static final double farRightVisibleX = -2.37;
+        public static final double bothCloseVisibleY = -17.69;
+        public static final double closeLeftVisibleY = -12.78;
+        public static final double closeRightVisibleY = -11.77;
+        public static final double farLeftVisibleX = 1.13;
+        public static final double farRightVisibleX = 4.47;
+        public static final double bothCloseHiddenErrorCheckY = -5.29;
         }
 
     public static final class OIConstants {
diff --git a/src/main/java/frc4388/robot/commands/auto/IdentifyPath.java b/src/main/java/frc4388/robot/commands/auto/IdentifyPath.java
index 681cd0c..e3f1bdc 100644
--- a/src/main/java/frc4388/robot/commands/auto/IdentifyPath.java
+++ b/src/main/java/frc4388/robot/commands/auto/IdentifyPath.java
@@ -7,6 +7,8 @@
 
 package frc4388.robot.commands.auto;
 
+import javax.lang.model.util.ElementScanner6;
+
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
@@ -20,6 +22,8 @@ public class IdentifyPath extends CommandBase {
   double yAngle;
   double target;
   public String path;
+  boolean closeVisible;
+  boolean finished;
 
   public IdentifyPath(LimeLight limeLight) {
     m_limeLight = limeLight;
@@ -32,6 +36,8 @@ public class IdentifyPath extends CommandBase {
   public void initialize() {
     m_limeLight.limeOn();
     path = "";
+    m_limeLight.changePipeline(1); //Dual Targetting Lowest
+    //closeVisible = false;
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -40,11 +46,21 @@ public class IdentifyPath extends CommandBase {
     target = m_limeLight.getV();
     xAngle = m_limeLight.getX();
     yAngle = m_limeLight.getY();
-    m_limeLight.changePipeline(1); //Dual Targetting
 
       //Identify which of four paths
-    if (withinError(yAngle, VisionConstants.bothCloseVisibleY)) //BLUE PATHS
+    if (withinError(yAngle, VisionConstants.bothCloseVisibleY) && !closeVisible) //BLUE PATHS
     {
+      closeVisible = true;
+    }
+    else if (!withinError(yAngle, VisionConstants.bothCloseVisibleY)&& !closeVisible) // RED PATHS
+    {
+      closeVisible = false;
+    }
+
+
+    if (closeVisible)
+    {
+      m_limeLight.changePipeline(2); //Dual Targetting Highest
       if(withinError(xAngle, VisionConstants.farLeftVisibleX)) //A PATH
       {
         path = "A_BLUE";
@@ -54,20 +70,22 @@ public class IdentifyPath extends CommandBase {
         path = "B_BLUE";
       }
     }
-
-    else // RED PATHS
+    else
     {
+      //m_limeLight.changePipeline(1); //Dual Targetting Lowest
       if(withinError(yAngle, VisionConstants.closeLeftVisibleY)) //A PATH
       {
         path = "A_RED";
       }
-      if(withinError(yAngle, VisionConstants.closeRightVisibleY)) //B PATH
+      else if(withinError(yAngle, VisionConstants.closeRightVisibleY)) //B PATH
       {
         path = "B_RED";
       }
     }
 
-    
+    System.out.println(path);
+    SmartDashboard.putString("GalacticSearchPath", path);
+    SmartDashboard.putBoolean("CloseVisible", closeVisible);
 
   }
 
diff --git a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
index a1e688e..3d0f9a0 100644
--- a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
@@ -59,6 +59,7 @@ public class TrackTarget extends CommandBase {
     // Vision Processing Mode
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(0);
   }
 
   // Called every time the scheduler runs while the command is scheduled.