From fd37bacd5a74e4213f1e4b7360b8600a51e1b3e2 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sun, 21 Mar 2021 17:29:31 -0600 Subject: [PATCH] GalacticSearch working Paths need to be drawn and tested --- src/main/java/frc4388/robot/Constants.java | 9 +-- .../java/frc4388/robot/RobotContainer.java | 4 +- .../robot/commands/auto/IdentifyPath.java | 74 +++++++++---------- .../frc4388/robot/subsystems/LimeLight.java | 4 +- 4 files changed, 44 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 2d1c58c..e59111a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -218,11 +218,10 @@ public final class Constants { //Galactic Search public static final double searchError = 0.5; 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 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 class OIConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3fa0e70..f1da19b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -282,9 +282,7 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> m_robotLime.limeOff())); new JoystickButton(m_joystick, 1) - .whenPressed(new IdentifyPath(m_robotLime)) - .whenReleased(new InstantCommand(() -> m_robotLime.limeOff())); - //.whileHeld(new RunCommand(() -> System.out.println("pog"))); + .whenPressed(new IdentifyPath(m_robotLime)); } public void buildAutos() { diff --git a/src/main/java/frc4388/robot/commands/auto/IdentifyPath.java b/src/main/java/frc4388/robot/commands/auto/IdentifyPath.java index e3f1bdc..44730f3 100644 --- a/src/main/java/frc4388/robot/commands/auto/IdentifyPath.java +++ b/src/main/java/frc4388/robot/commands/auto/IdentifyPath.java @@ -12,6 +12,7 @@ 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; +import edu.wpi.first.wpilibj2.command.InstantCommand; import frc4388.robot.Constants.VisionConstants; import frc4388.robot.subsystems.LimeLight; @@ -34,10 +35,8 @@ public class IdentifyPath extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { - m_limeLight.limeOn(); path = ""; - m_limeLight.changePipeline(1); //Dual Targetting Lowest - //closeVisible = false; + closeVisible = false; } // Called every time the scheduler runs while the command is scheduled. @@ -46,46 +45,46 @@ public class IdentifyPath extends CommandBase { target = m_limeLight.getV(); xAngle = m_limeLight.getX(); yAngle = m_limeLight.getY(); - - //Identify which of four 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 + m_limeLight.limeOn(); + //Identify which of four paths + m_limeLight.changePipeline(1);//Dual Targetting Lowest + if (withinError(yAngle, VisionConstants.bothCloseVisibleY) && !closeVisible) //BLUE PATHS { - path = "A_BLUE"; + closeVisible = true; } - if(withinError(xAngle, VisionConstants.farRightVisibleX)) //B PATH + else if (!withinError(yAngle, VisionConstants.bothCloseVisibleY) && !closeVisible) // RED PATHS { - path = "B_BLUE"; + closeVisible = false; } - } - 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"; - } - } + 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"; + } + } + + 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.putString("GalacticSearchPath", path); - SmartDashboard.putBoolean("CloseVisible", closeVisible); } @@ -112,6 +111,7 @@ public class IdentifyPath extends CommandBase { { SmartDashboard.putString("GalacticSearchPath", path); m_limeLight.galacticSearchPath = path; + m_limeLight.limeOff(); return true; } return false; diff --git a/src/main/java/frc4388/robot/subsystems/LimeLight.java b/src/main/java/frc4388/robot/subsystems/LimeLight.java index 68cba6c..7a5cc5c 100644 --- a/src/main/java/frc4388/robot/subsystems/LimeLight.java +++ b/src/main/java/frc4388/robot/subsystems/LimeLight.java @@ -21,8 +21,8 @@ public class LimeLight extends SubsystemBase { } public void limeOff(){ - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1); } public void limeOn(){