diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e59111a..9897059 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -216,12 +216,16 @@ public final class Constants { public static final double GRAV = 385.83; //Galactic Search - public static final double searchError = 0.5; - public static final double bothCloseVisibleY = -17.69; + public static final double searchError = 1; + /*public static final double bothCloseVisibleY = -17.69; public static final double closeLeftVisibleY = -12.57; 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 farRightVisibleX = 7.04;*/ + public static final double[] aRed = {-3.9, 0.8}; + public static final double[] bRed = {5.4, 5}; + public static final double[] aBlue = {9.27, 7}; + public static final double[] bBlue = {4.9, 8.6}; } public static final class OIConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 68b4da7..42c567a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -409,7 +409,7 @@ public class RobotContainer { //return m_barrel.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); //return m_barrelStart.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); //return m_sequentialTest.andThen(() -> m_robotDrive.tankDriveVelocity(0,0)); - return new IdentifyPath(m_robotLime).andThen(() -> m_galacticSearch.andThen(() -> m_robotDrive.tankDriveVelocity(0,0))); + return m_galacticSearch.andThen(() -> m_robotDrive.tankDriveVelocity(0,0)); } catch (Exception e) { System.err.println("ERROR"); diff --git a/src/main/java/frc4388/robot/commands/auto/GalacticSearch.java b/src/main/java/frc4388/robot/commands/auto/GalacticSearch.java index 9fc0b59..1574179 100644 --- a/src/main/java/frc4388/robot/commands/auto/GalacticSearch.java +++ b/src/main/java/frc4388/robot/commands/auto/GalacticSearch.java @@ -7,6 +7,7 @@ package frc4388.robot.commands.auto; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc4388.robot.commands.intake.RunIntake; @@ -21,21 +22,25 @@ public class GalacticSearch extends SequentialCommandGroup { * Creates a new GalacticSearch. */ public GalacticSearch(LimeLight m_limeLight, Intake m_intake, RamseteCommand[] paths) { - if (m_limeLight.galacticSearchPath == "A_RED") + addCommands( + paths[0] + ); + /* if (m_limeLight.galacticSearchPath == "A_RED") { - addCommands(new SequentialCommandGroup(paths[0], new RunIntake(m_intake))); + System.out.println(m_limeLight.galacticSearchPath +"YOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOO"); + addCommands(new ParallelCommandGroup(paths[0], new RunIntake(m_intake))); } else if (m_limeLight.galacticSearchPath == "A_BLUE") { - addCommands(new SequentialCommandGroup(paths[1], new RunIntake(m_intake))); + addCommands(new ParallelCommandGroup(paths[1], new RunIntake(m_intake))); } else if (m_limeLight.galacticSearchPath == "B_RED") { - addCommands(new SequentialCommandGroup(paths[2], new RunIntake(m_intake))); + addCommands(new ParallelCommandGroup(paths[2], new RunIntake(m_intake))); } else if (m_limeLight.galacticSearchPath == "B_BLUE") { - addCommands(new SequentialCommandGroup(paths[3], new RunIntake(m_intake))); - } + addCommands(new ParallelCommandGroup(paths[3], new RunIntake(m_intake))); + }*/ } } diff --git a/src/main/java/frc4388/robot/commands/auto/IdentifyPath.java b/src/main/java/frc4388/robot/commands/auto/IdentifyPath.java index 44730f3..4aff1c8 100644 --- a/src/main/java/frc4388/robot/commands/auto/IdentifyPath.java +++ b/src/main/java/frc4388/robot/commands/auto/IdentifyPath.java @@ -25,6 +25,7 @@ public class IdentifyPath extends CommandBase { public String path; boolean closeVisible; boolean finished; + boolean pathFound; public IdentifyPath(LimeLight limeLight) { m_limeLight = limeLight; @@ -37,6 +38,7 @@ public class IdentifyPath extends CommandBase { public void initialize() { path = ""; closeVisible = false; + pathFound = false; } // Called every time the scheduler runs while the command is scheduled. @@ -46,51 +48,92 @@ public class IdentifyPath extends CommandBase { xAngle = m_limeLight.getX(); yAngle = m_limeLight.getY(); m_limeLight.limeOn(); - //Identify which of four paths - m_limeLight.changePipeline(1);//Dual Targetting Lowest - if (withinError(yAngle, VisionConstants.bothCloseVisibleY) && !closeVisible) //BLUE PATHS - { - closeVisible = true; - } - else if (!withinError(yAngle, VisionConstants.bothCloseVisibleY) && !closeVisible) // RED PATHS - { - closeVisible = false; - } + // //Identify which of four paths + // m_limeLight.changePipeline(1);//Dual Targetting Lowest + // 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"; - } - if(withinError(xAngle, VisionConstants.farRightVisibleX)) //B PATH - { - path = "B_BLUE"; - } - } + // if (closeVisible) + // { + // m_limeLight.changePipeline(2); //Dual Targetting Highest + // if(withinError(xAngle, VisionConstants.farLeftVisibleX)) //A PATH + // { + // path = "A_BLUE"; + // } + // if(withinError(xAngle, VisionConstants.farRightVisibleX)) //B PATH + // { + // path = "B_BLUE"; + // } + // } - else{ - m_limeLight.changePipeline(1); //Dual Targetting Lowest - if(withinError(yAngle, VisionConstants.closeLeftVisibleY)) //A PATH - { - path = "A_RED"; - } - else if(withinError(yAngle, VisionConstants.closeRightVisibleY)) //B PATH - { - path = "B_RED"; - } - } + // else{ + // m_limeLight.changePipeline(1); //Dual Targetting Lowest + // if(withinError(yAngle, VisionConstants.closeLeftVisibleY)) //A PATH + // { + // path = "A_RED"; + // } + // else if(withinError(yAngle, VisionConstants.closeRightVisibleY)) //B PATH + // { + // path = "B_RED"; + // } + // } - SmartDashboard.putBoolean("CloseVisible", closeVisible); - System.out.println("If you see this message a bunch of times in a row, IdentifyPath.java is stuck trying to find the path for GalacticSearch"); - System.out.println(path); + SmartDashboard.putBoolean("PathFound", pathFound); + // System.out.println("If you see this message a bunch of times in a row, IdentifyPath.java is stuck trying to find the path for GalacticSearch"); + // System.out.println(path); + + + m_limeLight.changePipeline(1); + if (withinCoords(VisionConstants.aBlue)) + { + pathFound = true; + path = "A_BLUE"; + } + + m_limeLight.changePipeline(2); + if (withinCoords(VisionConstants.bBlue)) + { + pathFound = true; + path = "B_BLUE"; + } + + m_limeLight.changePipeline(1); + if (withinCoords(VisionConstants.aRed)) + { + pathFound = true; + path = "A_RED"; + } + + m_limeLight.changePipeline(1); + if (withinCoords(VisionConstants.bRed)) + { + pathFound = true; + path = "B_RED"; + } } - public boolean withinError(double angle, double input) + public boolean withinError(double angle, double goal) { - if(input > (angle - VisionConstants.searchError) && input < (angle + VisionConstants.searchError)) + if(goal > (angle - VisionConstants.searchError) && goal < (angle + VisionConstants.searchError)) + { + return true; + } + else + { + return false; + } + } + + public boolean withinCoords(double[] coords) + { + if (withinError(xAngle, coords[0]) && withinError(yAngle, coords[1])) { return true; }