Making Galactic Search work

untested

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

@@ -214,6 +214,14 @@ public final class Constants {
         public static final double DISTANCE_ERROR_EQUATION_M = 1.1279;
         public static final double DISTANCE_ERROR_EQUATION_B = -15.0684;
         public static final double GRAV = 385.83;
+
+            //Galactic Search
+        public static final double searchError = 0.5;
+        public static final double bothCloseVisibleY = -16.7;
+        public static final double closeLeftVisibleY = -11.47;
+        public static final double closeRightVisibleY = -10.50;
+        public static final double farLeftVisibleX = -1.03;
+        public static final double farRightVisibleX = -2.37;
         }
 
     public static final class OIConstants {

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

@@ -8,7 +8,6 @@
 package frc4388.robot;
 
 import java.nio.file.Path;
-import java.util.List;
 
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 
@@ -18,55 +17,44 @@ import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.controller.RamseteController;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Transform2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.trajectory.Trajectory;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.DriveConstants;
 import frc4388.robot.Constants.OIConstants;
-import frc4388.robot.commands.auto.DriveOffLineBackward;
-import frc4388.robot.commands.auto.DriveOffLineForward;
-import frc4388.robot.commands.auto.EightBallAutoMiddle;
-import frc4388.robot.commands.auto.FiveBallAutoMiddle;
-import frc4388.robot.commands.auto.SequentialTest;
-import frc4388.robot.commands.auto.SixBallAutoMiddle;
-import frc4388.robot.commands.auto.Slalom;
-import frc4388.robot.commands.auto.TankDriveVelocity;
-import frc4388.robot.commands.auto.TenBallAutoMiddle;
 import frc4388.robot.commands.InterruptSubystem;
-import frc4388.robot.commands.auto.AutoPath1FromCenter;
 import frc4388.robot.commands.auto.Barrel;
 import frc4388.robot.commands.auto.BarrelMany;
 import frc4388.robot.commands.auto.BarrelStart;
 import frc4388.robot.commands.auto.Bounce;
+import frc4388.robot.commands.auto.DriveOffLineBackward;
+import frc4388.robot.commands.auto.DriveOffLineForward;
+import frc4388.robot.commands.auto.EightBallAutoMiddle;
+import frc4388.robot.commands.auto.FiveBallAutoMiddle;
+import frc4388.robot.commands.auto.GalacticSearch;
+import frc4388.robot.commands.auto.IdentifyPath;
+import frc4388.robot.commands.auto.SequentialTest;
+import frc4388.robot.commands.auto.SixBallAutoMiddle;
+import frc4388.robot.commands.auto.Slalom;
+import frc4388.robot.commands.auto.TenBallAutoMiddle;
 import frc4388.robot.commands.auto.Wait;
 import frc4388.robot.commands.climber.DisengageRachet;
 import frc4388.robot.commands.climber.RunClimberWithTriggers;
 import frc4388.robot.commands.climber.RunLevelerWithJoystick;
 import frc4388.robot.commands.drive.DriveStraightAtVelocityPID;
-import frc4388.robot.commands.drive.DriveStraightToPositionMM;
 import frc4388.robot.commands.drive.DriveWithJoystick;
 import frc4388.robot.commands.drive.TurnDegrees;
 import frc4388.robot.commands.intake.RunIntakeWithTriggers;
 import frc4388.robot.commands.shooter.CalibrateShooter;
-import frc4388.robot.commands.shooter.TrackTarget;
-import frc4388.robot.commands.shooter.TrimShooter;
 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.StoragePrep;
-import frc4388.robot.commands.storage.ManageStorage.StorageMode;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Climber;
 import frc4388.robot.subsystems.Drive;
@@ -110,6 +98,7 @@ public class RobotContainer {
     /* Controllers */
     private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
     private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
+    private final Joystick m_joystick = new Joystick(0);
 
     /* Autos */
     double m_totalTimeAuto;
@@ -142,6 +131,8 @@ public class RobotContainer {
 
     SequentialTest m_sequentialTest;
 
+    GalacticSearch m_galacticSearch;
+
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
@@ -289,6 +280,11 @@ public class RobotContainer {
             .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
             //.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET))
             .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")));
     }
 
     public void buildAutos() {
@@ -379,6 +375,14 @@ public class RobotContainer {
         };
 
         m_sequentialTest = new SequentialTest(this, buildPaths(sequentialTestPaths));
+
+        String[] galacticSearchPaths = new String[]{
+            "aRed",
+            "aBlue",
+            "bRed",
+            "bBlue"
+        };
+        m_galacticSearch = new GalacticSearch(m_robotLime, buildPaths(galacticSearchPaths));
     }
 
     /**
@@ -405,8 +409,9 @@ public class RobotContainer {
             //return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_slalom.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_barrel.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
-            return m_barrelStart.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)));
 
         } catch (Exception e) {
             System.err.println("ERROR");

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

@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+import frc4388.robot.subsystems.LimeLight;
+
+// 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 GalacticSearch extends SequentialCommandGroup {
+  /**
+   * Creates a new GalacticSearch.
+   */
+  public GalacticSearch(LimeLight m_limeLight, RamseteCommand[] paths) {
+    if (m_limeLight.galacticSearchPath == "A_RED")
+    {
+      addCommands(paths[0]);
+    }
+    else if (m_limeLight.galacticSearchPath == "A_BLUE")
+    {
+      addCommands(paths[1]);
+    }
+    else if (m_limeLight.galacticSearchPath == "B_RED")
+    {
+      addCommands(paths[2]);
+    }
+    else if (m_limeLight.galacticSearchPath == "B_BLUE")
+    {
+      addCommands(paths[3]);
+    }
+  }
+}

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

@@ -0,0 +1,101 @@
+/*----------------------------------------------------------------------------*/
+/* 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.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.VisionConstants;
+import frc4388.robot.subsystems.LimeLight;
+
+public class IdentifyPath extends CommandBase {
+
+  LimeLight m_limeLight;
+  double xAngle;
+  double yAngle;
+  double target;
+  public String path;
+
+  public IdentifyPath(LimeLight limeLight) {
+    m_limeLight = limeLight;
+    addRequirements(m_limeLight);
+    m_limeLight.limeOff();
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_limeLight.limeOn();
+    path = "";
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    target = m_limeLight.getV();
+    xAngle = m_limeLight.getX();
+    yAngle = m_limeLight.getY();
+    m_limeLight.changePipeline(1); //Dual Targetting
+
+      //Identify which of four paths
+    if (withinError(yAngle, VisionConstants.bothCloseVisibleY)) //BLUE PATHS
+    {
+      if(withinError(xAngle, VisionConstants.farLeftVisibleX)) //A PATH
+      {
+        path = "A_BLUE";
+      }
+      if(withinError(xAngle, VisionConstants.farRightVisibleX)) //B PATH
+      {
+        path = "B_BLUE";
+      }
+    }
+
+    else // RED PATHS
+    {
+      if(withinError(yAngle, VisionConstants.closeLeftVisibleY)) //A PATH
+      {
+        path = "A_RED";
+      }
+      if(withinError(yAngle, VisionConstants.closeRightVisibleY)) //B PATH
+      {
+        path = "B_RED";
+      }
+    }
+
+    
+
+  }
+
+  public boolean withinError(double angle, double input)
+  {
+    if(input > (angle - VisionConstants.searchError) && input < (angle + VisionConstants.searchError))
+    {
+      return true;
+    }
+    else
+    {
+      return false;
+    }
+  }
+  // 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() {
+    if (path != "")
+    {
+      SmartDashboard.putString("GalacticSearchPath", path);
+      m_limeLight.galacticSearchPath = path;
+      return true;
+    }
+    return false;
+  }
+}

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

@@ -14,6 +14,8 @@ public class LimeLight extends SubsystemBase {
   /**
    * Creates a new LimeLight.
    */
+  public String galacticSearchPath;
+
   public LimeLight() {
 
   }
@@ -27,6 +29,24 @@ public class LimeLight extends SubsystemBase {
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
   }
+  
+  public void changePipeline(int pipelineId)
+  {
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(pipelineId);
+  }
+
+  public double getV()
+  {
+    return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0);
+  }
+  public double getX()
+  {
+    return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
+  }
+  public double getY()
+  {
+    return NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
+  }
 
   @Override
   public void periodic() {