changes to paths

src/main/java/frc4388/robot/commands/auto/ExecuteCommand.java

@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.auto;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import frc4388.robot.subsystems.LimeLight;
+
+public class ExecuteCommand extends CommandBase {
+  /**
+   * Creates a new ExecuteCommand.
+   */
+  RamseteCommand[] m_paths;
+  LimeLight m_limeLight;
+  public ExecuteCommand(LimeLight limeLight, RamseteCommand[] paths) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_limeLight = limeLight;
+    m_paths = paths;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    String gsPath = m_limeLight.galacticSearchPath;
+    switch (gsPath)
+    {
+      case "A_RED":
+        new RunPath(m_paths[0]);
+        break;
+      case "A_BLUE":
+        new RunPath(m_paths[1]);
+        break;
+      case "B_RED":
+        new RunPath(m_paths[2]);
+        break;
+      case "B_BLUE":
+        new RunPath(m_paths[3]);
+        break;
+      case "test":
+        new RunPath(m_paths[0]);
+        break;
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc4388/robot/commands/auto/GalacticSearch.java

@@ -7,6 +7,7 @@
 
 package frc4388.robot.commands.auto;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -22,12 +23,12 @@ public class GalacticSearch extends SequentialCommandGroup {
    * Creates a new GalacticSearch.
    */
   public GalacticSearch(LimeLight m_limeLight, Intake m_intake, RamseteCommand[] paths) {
-    addCommands(
-      paths[0]
-    );
-   /* if (m_limeLight.galacticSearchPath == "A_RED")
+    // addCommands(
+    //   new IdentifyPath(m_limeLight),
+    //   new ExecuteCommand(m_limeLight, paths)
+    // );
+   if (m_limeLight.galacticSearchPath == "A_RED")
     {
-      System.out.println(m_limeLight.galacticSearchPath +"YOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOO");
       addCommands(new ParallelCommandGroup(paths[0], new RunIntake(m_intake)));
     }
     else if (m_limeLight.galacticSearchPath == "A_BLUE")
@@ -41,6 +42,10 @@ public class GalacticSearch extends SequentialCommandGroup {
     else if (m_limeLight.galacticSearchPath == "B_BLUE")
     {
       addCommands(new ParallelCommandGroup(paths[3], new RunIntake(m_intake)));
-    }*/
+    }
+    else if (m_limeLight.galacticSearchPath == "test")
+    {
+      addCommands(new ParallelCommandGroup(paths[0], new RunIntake(m_intake)));
+    }
   }
 }

src/main/java/frc4388/robot/commands/auto/IdentifyPath.java

@@ -47,7 +47,7 @@ public class IdentifyPath extends CommandBase {
     target = m_limeLight.getV();
     xAngle = m_limeLight.getX();
     yAngle = m_limeLight.getY();
-    m_limeLight.limeOn();
+    //m_limeLight.limeOn();
     //     //Identify which of four paths
     //   m_limeLight.changePipeline(1);//Dual Targetting Lowest
     //   if (withinError(yAngle, VisionConstants.bothCloseVisibleY) && !closeVisible) //BLUE PATHS
@@ -117,6 +117,7 @@ public class IdentifyPath extends CommandBase {
       path = "B_RED";
     }
 
+    path = "test";
   }
 
   public boolean withinError(double angle, double goal)
@@ -154,7 +155,7 @@ public class IdentifyPath extends CommandBase {
     {
       SmartDashboard.putString("GalacticSearchPath", path);
       m_limeLight.galacticSearchPath = path;
-      m_limeLight.limeOff();
+      //m_limeLight.limeOff();
       return true;
     }
     return false;

src/main/java/frc4388/robot/commands/auto/RunPath.java

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.auto;
+
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class RunPath extends SequentialCommandGroup {
+  /**
+   * Creates a new RunPath.
+   */
+  public RunPath(RamseteCommand path) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    addCommands(
+      path
+    );
+  }
+}