Merge branch 'GalacticSearch' of https://github.com/Team4388/RiseOfRidgebotics2020 into GalacticSearch

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

@@ -222,10 +222,12 @@ public final class Constants {
         public static final double closeRightVisibleY = -11.35;
         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[] bRed = {-7.6, 4.3};
         public static final double[] aBlue = {9.9, 8.2};
         public static final double[] bBlue = {4.8, 8.8};
+
         }
 
     public static final class OIConstants {

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

@@ -82,6 +82,7 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void autonomousInit() {
+    //m_robotContainer.buildAutos();
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
@@ -105,6 +106,9 @@ public class Robot extends TimedRobot {
       m_autonomousCommand.schedule();
       SmartDashboard.putString("Is Auto Start?", "YEA");
     }
+    else{
+
+    }
   }
 
   /**

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

@@ -100,6 +100,8 @@ public class RobotContainer {
     private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
     private final Joystick m_joystick = new Joystick(0);
 
+    public boolean isGS = false;
+
     /* Autos */
     double m_totalTimeAuto;
 
@@ -286,100 +288,13 @@ public class RobotContainer {
     }
 
     public void buildAutos() {
-        String[] sixBallAutoMiddlePaths = new String[]{
-            "SixBallMidComplete"
-        };
-
-        m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths));
-
-        String[] sixBallAutoMiddle0Paths = new String[]{
-            "SixBallMid0"
-        };
-
-        m_sixBallAutoMiddle0 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle0Paths));
-
-        String[] sixBallAutoMiddle1Paths = new String[]{
-            "SixBallMid1"
-        };
-
-        m_sixBallAutoMiddle1 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle1Paths));
-
-        String[] slalom = new String[]{
-            "Slalom"
-        };
-
-        m_slalom = new Slalom(m_robotDrive, buildPaths(slalom));
-
-        String[] barrel = new String[]{
-            "Barrel"
-        };
-
-        m_barrel = new Barrel(m_robotDrive, buildPaths(barrel));
-        
-        String[] barrelStart = new String[]{
-            "BarrelStart"
-        };
-
-        m_barrelStart = new BarrelStart(m_robotDrive, buildPaths(barrelStart));
-
-        String[] bounce = new String[]{
-            "Bounce1",
-            "Bounce2",
-            "Bounce3",
-            "Bounce4"
-        };
-
-        m_bounce = new Bounce(m_robotDrive, buildPaths(bounce));
-        
-        String[] barrelMany = new String[]{
-            "BarrelManyWaypoints"
-        };
-
-        m_barrelMany = new BarrelMany(m_robotDrive, buildPaths(barrelMany));
-        
-        String[] eightBallAutoMiddlePaths = new String[]{
-            "EightBallMidComplete"
-        };
-
-        m_eightBallAutoMiddle = new EightBallAutoMiddle(m_robotDrive, buildPaths(eightBallAutoMiddlePaths));
-
-        String[] driveOffLineForwardPaths = new String[]{
-            "DriveOffLineForward"
-        };
-
-        m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, buildPaths(driveOffLineForwardPaths));
-
-        String[] driveOffLineBackwardPaths = new String[]{
-            "DriveOffLineBackward"
-        };
-
-        m_driveOffLineBackward = new DriveOffLineBackward(m_robotDrive, buildPaths(driveOffLineBackwardPaths));
-        
-        String[] fiveBallAutoMiddlePaths = new String[]{
-            "FiveBallMidComplete"
-        };
-
-        m_fiveBallAutoMiddle = new FiveBallAutoMiddle(m_robotDrive, buildPaths(fiveBallAutoMiddlePaths));
-
-        String[] tenBallAutoMiddlePaths = new String[]{
-            "TenBallMidComplete"
-        };
-
-        m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotDrive, buildPaths(tenBallAutoMiddlePaths));
-    
-        String[] sequentialTestPaths = new String[]{
-            "Seq1",
-            "Seq2"
-        };
-
-        m_sequentialTest = new SequentialTest(this, buildPaths(sequentialTestPaths));
-
         String[] galacticSearchPaths = new String[]{
-            "aRed",
-            "aBlue",
-            "bRed",
-            "bBlue"
+            "GSC_ARED",
+            "GSC_ABLUE",
+            "GSC_BRED",
+            "GSC_BBLUE"
         };
+        m_robotLime.identifyPath();
         m_galacticSearch = new GalacticSearch(m_robotLime, m_robotIntake, buildPaths(galacticSearchPaths));
     }
 
@@ -397,7 +312,6 @@ public class RobotContainer {
         // Run path following command, then stop at the end.
         try {
             SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto);
-
             //return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_sixBallAutoMiddle1.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));

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
+    );
+  }
+}

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

@@ -8,7 +8,9 @@
 package frc4388.robot.subsystems;
 
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.VisionConstants;
 
 public class LimeLight extends SubsystemBase {
   /**
@@ -48,6 +50,57 @@ public class LimeLight extends SubsystemBase {
     return NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
   }
 
+  public void identifyPath(){
+    changePipeline(1);
+    if (withinCoords(VisionConstants.aBlue))
+    {
+      galacticSearchPath = "A_BLUE";
+    }
+
+    changePipeline(2);
+    if (withinCoords(VisionConstants.bBlue))
+    {
+      galacticSearchPath = "B_BLUE";
+    }
+
+    changePipeline(1);
+    if (withinCoords(VisionConstants.aRed))
+    {
+      galacticSearchPath = "A_RED";
+    }
+
+    changePipeline(1);
+    if (withinCoords(VisionConstants.bRed))
+    {
+      galacticSearchPath = "B_RED";
+    }
+    SmartDashboard.putString("GalacticSearchPath", galacticSearchPath);
+  }
+
+public boolean withinError(double angle, double goal)
+{
+  if(goal > (angle - VisionConstants.searchError) && goal < (angle + VisionConstants.searchError))
+  {
+    return true;
+  }
+  else
+  {
+    return false;
+  }
+}
+
+public boolean withinCoords(double[] coords)
+{
+  if (withinError(getX(), coords[0]) && withinError(getY(), coords[1]))
+  {
+    return true;
+  }
+  else
+  {
+    return false;
+  }
+}
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run