From d9fc99ba4ae18990321da0e457b7828e1b6ad448 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 19 Mar 2021 01:34:57 -0600 Subject: [PATCH] Making Galactic Search work untested --- src/main/java/frc4388/robot/Constants.java | 8 ++ .../java/frc4388/robot/RobotContainer.java | 51 +++++---- .../robot/commands/auto/GalacticSearch.java | 39 +++++++ .../robot/commands/auto/IdentifyPath.java | 101 ++++++++++++++++++ .../frc4388/robot/subsystems/LimeLight.java | 20 ++++ 5 files changed, 196 insertions(+), 23 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/auto/GalacticSearch.java create mode 100644 src/main/java/frc4388/robot/commands/auto/IdentifyPath.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7443120..16da8d0 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/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 { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 373f563..3fa0e70 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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"); diff --git a/src/main/java/frc4388/robot/commands/auto/GalacticSearch.java b/src/main/java/frc4388/robot/commands/auto/GalacticSearch.java new file mode 100644 index 0000000..357fb20 --- /dev/null +++ b/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]); + } + } +} diff --git a/src/main/java/frc4388/robot/commands/auto/IdentifyPath.java b/src/main/java/frc4388/robot/commands/auto/IdentifyPath.java new file mode 100644 index 0000000..681cd0c --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/LimeLight.java b/src/main/java/frc4388/robot/subsystems/LimeLight.java index d8e39a4..68cba6c 100644 --- a/src/main/java/frc4388/robot/subsystems/LimeLight.java +++ b/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() {