GSC

src/main/java/frc4388/robot/Constants.java

@@ -223,9 +223,9 @@ public final class Constants {
         public static final double farLeftVisibleX = 3.58;
         public static final double farRightVisibleX = 7.04;*/
 
-        public static final double[] aRed = {3.8, 0.3};
+        public static final double[] aRed = {-2.3, 0.6};
         public static final double[] bRed = {-7.6, 4.3};
-        public static final double[] aBlue = {9.9, 8.2};
+        public static final double[] aBlue = {9.9, 9.0};
         public static final double[] bBlue = {4.8, 8.8};
 
         }

src/main/java/frc4388/robot/Robot.java

@@ -75,6 +75,7 @@ public class Robot extends TimedRobot {
   @Override
   public void disabledPeriodic() {
     m_robotContainer.resetOdometry(new Pose2d());
+    m_robotContainer.idenPath();
   }
 
   /**

src/main/java/frc4388/robot/RobotContainer.java

@@ -55,6 +55,8 @@ import frc4388.robot.commands.shooter.CalibrateShooter;
 import frc4388.robot.commands.shooter.ShootPrepGroup;
 import frc4388.robot.commands.shooter.TrackTarget;
 import frc4388.robot.commands.shooter.TrimShooter;
+import frc4388.robot.commands.storage.ManageStorage;
+import frc4388.robot.commands.storage.ManageStorage.StorageMode;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Climber;
 import frc4388.robot.subsystems.Drive;
@@ -173,8 +175,8 @@ public class RobotContainer {
         // continually sends updates to the Blinkin LED controller to keep the lights on
         m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
         // runs the storage not
-        m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
-        //m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage, StorageMode.IDLE));
+        //m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
+        m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage, StorageMode.IDLE));
         //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
     }
 
@@ -294,10 +296,14 @@ public class RobotContainer {
             "GSC_BRED",
             "GSC_BBLUE"
         };
-        m_robotLime.identifyPath();
+        idenPath();
         m_galacticSearch = new GalacticSearch(m_robotLime, m_robotIntake, buildPaths(galacticSearchPaths));
     }
 
+    public void idenPath()
+    {
+        m_robotLime.identifyPath();
+    }
     /**
      * Use this to pass the autonomous command to the main {@link Robot} class.
      *
@@ -308,7 +314,6 @@ public class RobotContainer {
         //TrajectoryConfig config = getTrajectoryConfig();
         //Trajectory trajectory = getTrajectory(config);
         //RamseteCommand ramseteCommand = getRamseteCommand(trajectory);
-
         // Run path following command, then stop at the end.
         try {
             SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto);

src/main/java/frc4388/robot/subsystems/LimeLight.java

@@ -50,31 +50,58 @@ public class LimeLight extends SubsystemBase {
     return NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
   }
 
+  int i = 0;
+  boolean onceThrough = false;
+  boolean pathFound = false;
   public void identifyPath(){
-    changePipeline(1);
-    if (withinCoords(VisionConstants.aBlue))
-    {
-      galacticSearchPath = "A_BLUE";
-    }
 
-    changePipeline(2);
-    if (withinCoords(VisionConstants.bBlue))
+    if (!onceThrough)
     {
-      galacticSearchPath = "B_BLUE";
-    }
+      changePipeline(2);
+      if (withinCoords(VisionConstants.aBlue))
+      {
+        pathFound = true;
+        galacticSearchPath = "A_BLUE";
+      }
 
-    changePipeline(1);
-    if (withinCoords(VisionConstants.aRed))
-    {
-      galacticSearchPath = "A_RED";
-    }
+      changePipeline(4);
+      if (withinCoords(VisionConstants.bBlue))
+      {
+        pathFound = true;
+        galacticSearchPath = "B_BLUE";
+      }
 
-    changePipeline(1);
-    if (withinCoords(VisionConstants.bRed))
-    {
-      galacticSearchPath = "B_RED";
+      changePipeline(1);
+      if (withinCoords(VisionConstants.aRed))
+      {
+        pathFound = true;
+        galacticSearchPath = "A_RED";
+      }
+
+      changePipeline(3);
+      if (withinCoords(VisionConstants.bRed))
+      {
+        pathFound = true;
+        galacticSearchPath = "B_RED";
+      }
+
+      if (pathFound == false)
+      {
+        galacticSearchPath = "PathNotFound";
+      }
+      SmartDashboard.putString("GalacticSearchPath", galacticSearchPath);
+      onceThrough = true;
+    }
+    else{
+      i++;
+      SmartDashboard.putNumber("Counter", i);
+    }
+    if (i >= 50)
+    {
+      i=0;
+      pathFound = false;
+      onceThrough = false;
     }
-    SmartDashboard.putString("GalacticSearchPath", galacticSearchPath);
   }
 
 public boolean withinError(double angle, double goal)