diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 1a270763..b802f85b 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -36,6 +36,8 @@ public static class ControlConstants { private final CommandXboxController driveController; private final CommandXboxController codriveController; + // leaving this code in in case we need to test outside of auto + // private final Trigger getWithinDistanceTrigger; // Intake private final Trigger driveIntakeInButton; @@ -59,6 +61,9 @@ public static class ControlConstants { public Controls(Subsystems s) { driveController = new CommandXboxController(CONTROLLER_PORT); codriveController = new CommandXboxController(CODRIVER_CONTROLLER_PORT); + // not sure what drive team wants (or if the trigger is even needed since we are only using the + // command in auto) + // getWithinDistanceTrigger = driveController.start(); this.s = s; launcherAmpPresetButton = codriveController.x(); @@ -84,6 +89,12 @@ public Controls(Subsystems s) { if (DRIVEBASE_ENABLED) { bindDrivebaseControls(); } + // leaving this code in in case we need to test outside of auto + /* + if (LIMELIGHT_ENABLED) { + bindLimelightControls(); + } + */ if (LAUNCHER_ENABLED) { bindLauncherControls(); } @@ -116,9 +127,9 @@ private void bindDrivebaseControls() { driveController::getLeftY, driveController::getLeftX, () -> Rotation2d.fromRotations(driveController.getRightX()))); - driveController.start().onTrue(new InstantCommand(s.drivebaseSubsystem::resetGyro)); driveController.rightStick().onTrue(new InstantCommand(s.drivebaseSubsystem::toggleXWheels)); - // driveController x + driveController.start().onTrue(new InstantCommand(s.drivebaseSubsystem::resetGyro)); + // driveController // .back() // .onTrue( // new InstantCommand( @@ -127,6 +138,14 @@ private void bindDrivebaseControls() { // new Pose2d(new Translation2d(1.3, 5.55), new Rotation2d(180))))); } + // leaving this code in in case we need to test outside of auto + /* + public void bindLimelightControls() { + getWithinDistanceTrigger.whileTrue( + new DriveToNoteCommand(s.drivebaseSubsystem, s.limelightSubsystem)); + } + */ + // intake controls private void bindIntakeControls() { // CommandScheduler.getInstance() diff --git a/src/main/java/frc/team2412/robot/Subsystems.java b/src/main/java/frc/team2412/robot/Subsystems.java index c71fbdd1..5b4f182d 100644 --- a/src/main/java/frc/team2412/robot/Subsystems.java +++ b/src/main/java/frc/team2412/robot/Subsystems.java @@ -6,13 +6,13 @@ import frc.team2412.robot.subsystems.DrivebaseSubsystem; import frc.team2412.robot.subsystems.IntakeSubsystem; import frc.team2412.robot.subsystems.LauncherSubsystem; +import frc.team2412.robot.subsystems.LimelightSubsystem; import frc.team2412.robot.util.DrivebaseWrapper; public class Subsystems { public static class SubsystemConstants { private static final boolean IS_COMP = Robot.getInstance().isCompetition(); - public static final boolean APRILTAGS_ENABLED = true; public static final boolean LIMELIGHT_ENABLED = false; public static final boolean CLIMB_ENABLED = false; @@ -26,6 +26,7 @@ public static class SubsystemConstants { public final DrivebaseWrapper drivebaseWrapper; public final DrivebaseSubsystem drivebaseSubsystem; public final LauncherSubsystem launcherSubsystem; + public final LimelightSubsystem limelightSubsystem; public final IntakeSubsystem intakeSubsystem; public final AprilTagsProcessor apriltagsProcessor; @@ -48,6 +49,11 @@ public Subsystems() { } else { launcherSubsystem = null; } + if (LIMELIGHT_ENABLED) { + limelightSubsystem = new LimelightSubsystem(); + } else { + limelightSubsystem = null; + } if (INTAKE_ENABLED) { intakeSubsystem = new IntakeSubsystem(); } else { diff --git a/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java b/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java new file mode 100644 index 00000000..933117c2 --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java @@ -0,0 +1,44 @@ +package frc.team2412.robot.commands.drivebase; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import frc.team2412.robot.subsystems.DrivebaseSubsystem; +import frc.team2412.robot.subsystems.LimelightSubsystem; + +public class DriveToNoteCommand extends Command { + + private final DrivebaseSubsystem drivebaseSubsystem; + private final LimelightSubsystem limelightSubsystem; + + // limelight placement might be different so this multiplier is convenient + private final double INVERT_DRIVE_DIRECTION = -1.0; + + public DriveToNoteCommand( + DrivebaseSubsystem drivebaseSubsystem, LimelightSubsystem limelightSubsystem) { + this.drivebaseSubsystem = drivebaseSubsystem; + this.limelightSubsystem = limelightSubsystem; + addRequirements(drivebaseSubsystem); + } + + @Override + public void execute() { + if (limelightSubsystem.hasTargets()) { + Translation2d move = + new Translation2d( + INVERT_DRIVE_DIRECTION + * Units.inchesToMeters(limelightSubsystem.getDistanceFromTargetInches()), + 0.0); + Rotation2d turn = + new Rotation2d() + .fromDegrees(2 * INVERT_DRIVE_DIRECTION * limelightSubsystem.getHorizontalOffset()); + drivebaseSubsystem.drive(move, turn, false); + } + } + + @Override + public boolean isFinished() { + return (limelightSubsystem.isWithinDistance() || !limelightSubsystem.hasTargets()); + } +} diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 466f8342..e57a70b3 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -173,8 +173,7 @@ public void drive(Translation2d translation, Rotation2d rotation, boolean fieldO // if we're requesting the robot to stay still, lock wheels in X formation if (translation.getNorm() == 0 && rotation.getRotations() == 0 && xWheelsEnabled) { swerveDrive.lockPose(); - } - if (rotationSetpoint != null) { + } else if (rotationSetpoint != null) { swerveDrive.drive( translation.unaryMinus(), rotationSetpoint.getRadians(), fieldOriented, false); } else { diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java new file mode 100644 index 00000000..a3109e6a --- /dev/null +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -0,0 +1,112 @@ +package frc.team2412.robot.subsystems; + +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.Map; + +public class LimelightSubsystem extends SubsystemBase { + + // POTENTIAL CONSTANTS + private GenericEntry GOAL_DISTANCE_FROM_NOTE; + + // MEMBERS + + final NetworkTable networkTable; + + String currentPoseString; + + // network tables + + // CONSTRUCTOR ! + public LimelightSubsystem() { + + // camera stream at http://10.24.12.11:5800 + + // logging + currentPoseString = ""; + + networkTable = NetworkTableInstance.getDefault().getTable("limelight"); + ShuffleboardTab limelightTab = Shuffleboard.getTab("Limelight"); + + // helpers + setPipeline(); + + limelightTab.addBoolean("hasTarget", this::hasTargets).withPosition(0, 0).withSize(1, 1); + limelightTab + .addDouble("Horizontal Offset", this::getHorizontalOffset) + .withPosition(1, 0) + .withSize(1, 1); + limelightTab + .addDouble("Vertical Offset", this::getVerticalOffset) + .withPosition(2, 0) + .withSize(1, 1); + limelightTab + .addDouble("Distance From Note", this::getDistanceFromTargetInches) + .withPosition(3, 0) + .withSize(1, 1); + limelightTab + .addString("Current Pose ", this::getCurrentPoseString) + .withPosition(0, 1) + .withSize(4, 1); + GOAL_DISTANCE_FROM_NOTE = + limelightTab + .addPersistent("Goal distance", 20.0) + .withWidget(BuiltInWidgets.kNumberSlider) + .withPosition(4, 0) + .withSize(2, 1) + .withProperties(Map.of("Min", 0.0, "Max", 30.0)) + .getEntry(); + } + + private void setPipeline() { + networkTable.getEntry("pipeline").setNumber(0); + } + + // METHODS + + public boolean hasTargets() { + return (networkTable.getEntry("tv").getDouble(0) != 0); + } + + public double getHorizontalOffset() { + return networkTable.getEntry("tx").getDouble(0); + } + + public double getVerticalOffset() { + return networkTable.getEntry("ty").getDouble(0); + } + + public double getBoxWidth() { + return networkTable.getEntry("tlong").getDouble(0); + } + // TODO re-measure distances to make this right + public double getDistanceFromTargetInches() { + + // focal length = (P x D) / W + double focal_length = 559.0394; + + // distance = (W x F) / P + // returns inches for testing purposes, will divide by 39.3700787 to return meters + if (hasTargets()) { + return (14 * focal_length) / getBoxWidth(); + } else { + return 0.0; + } + } + + public String getCurrentPoseString() { + return currentPoseString; + } + + public boolean isWithinDistance() { + return (getDistanceFromTargetInches() <= GOAL_DISTANCE_FROM_NOTE.getDouble(20.0)); + } + + @Override + public void periodic() {} +}