From 11e4908055a5d2d305bfcb06bff345d8536d088f Mon Sep 17 00:00:00 2001 From: Jonah <58869582+jbko6@users.noreply.github.com> Date: Tue, 9 Jan 2024 17:45:10 -0800 Subject: [PATCH 1/4] limelight subsystem added --- .../limelight/GetWithinDistanceCommand.java | 29 +++++++++ .../robot/subsystems/LimelightSubsystem.java | 62 +++++++++++++++++++ .../robot/subsystems/VisionSubsystem.java | 2 + 3 files changed, 93 insertions(+) create mode 100644 src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java create mode 100644 src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java diff --git a/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java b/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java new file mode 100644 index 00000000..c8358469 --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java @@ -0,0 +1,29 @@ +package frc.team2412.robot.commands.limelight; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.team2412.robot.subsystems.DrivebaseSubsystem; +import frc.team2412.robot.subsystems.LimelightSubsystem; + +public class GetWithinDistanceCommand extends CommandBase { + + private LimelightSubsystem limelightSubsystem; + private DrivebaseSubsystem drivebaseSubsystem; + + public GetWithinDistanceCommand( + LimelightSubsystem limelightSubsystem, DrivebaseSubsystem drivebaseSubsystem) { + this.limelightSubsystem = limelightSubsystem; + this.drivebaseSubsystem = drivebaseSubsystem; + } + + public void execute() { + // TOOD: use driver assist code + // get drivebase pose -> feed into get pose method -> use driver assist code + + Pose2d currentPose = drivebaseSubsystem.getPose(); + + // driver assist code + + limelightSubsystem.getWithinDistance(currentPose); + } +} 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..98faa5bf --- /dev/null +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -0,0 +1,62 @@ +package frc.team2412.robot.subsystems; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class LimelightSubsystem extends SubsystemBase { + + // CONSTANTS + + // meters? + public static final double CAMERA_HEIGHT = 0.2; + public static final double CAMERA_ANGLE_OFFSET = 0; + + public static final double GOAL_DISTANCE_FROM_TARGET = 0.3; + public static final double GOAL_DISTANCE_FROM_CONE = 0.3; + public static final double GOAL_DISTANCE_FROM_CUBE = 0.3; + + // MEMBERS + + NetworkTable networkTable; + + // network tables + + // CONSTRUCTOR ! + public LimelightSubsystem() { + + networkTable = NetworkTableInstance.getDefault().getTable("limelight"); + } + + // 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 getDistanceFromTarget() { + double targetHeight = 0; + double angleToTarget = 0; + return (targetHeight - CAMERA_HEIGHT) / Math.tan(CAMERA_ANGLE_OFFSET + angleToTarget); + } + + public Pose2d getTargetPose(Pose2d currentPose) { + + // math thing to get target pose using current pose + + return currentPose; // replace later + } + + public void getWithinDistance(Pose2d currentPose) { + getTargetPose(currentPose); + } +} diff --git a/src/main/java/frc/team2412/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/VisionSubsystem.java index 9811b797..13c40c3f 100644 --- a/src/main/java/frc/team2412/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/VisionSubsystem.java @@ -284,4 +284,6 @@ public boolean isYawAlignedToGrid() { public void setAlliance(DriverStation.Alliance alliance) { this.alliance = alliance; } + + // } From 6f0b185dad05a24d2c04c6b6e72a2086b4a21586 Mon Sep 17 00:00:00 2001 From: Jonah <58869582+jbko6@users.noreply.github.com> Date: Tue, 9 Jan 2024 20:25:43 -0800 Subject: [PATCH 2/4] command added --- .../java/frc/team2412/robot/Controls.java | 14 ++ .../java/frc/team2412/robot/Subsystems.java | 12 +- .../limelight/GetWithinDistanceCommand.java | 28 +++- .../robot/subsystems/LimelightSubsystem.java | 123 +++++++++++++++++- 4 files changed, 169 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 6c71704b..44e3a065 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -24,6 +24,7 @@ import frc.team2412.robot.commands.intake.IntakeSetOutCommand; import frc.team2412.robot.commands.led.LEDPurpleCommand; import frc.team2412.robot.commands.led.LEDYellowCommand; +import frc.team2412.robot.commands.limelight.GetWithinDistanceCommand; import frc.team2412.robot.subsystems.IntakeSubsystem.IntakeConstants.GamePieceType; import frc.team2412.robot.util.DriverAssist; @@ -69,6 +70,8 @@ public static class ControlConstants { public final Trigger ledPurple; public final Trigger ledYellow; + public final Trigger getWithinDistanceTrigger; + private final Subsystems s; public Controls(Subsystems s) { @@ -100,6 +103,8 @@ public Controls(Subsystems s) { ledPurple = codriveController.rightBumper(); ledYellow = codriveController.leftBumper(); + getWithinDistanceTrigger = driveController.start(); + if (Subsystems.SubsystemConstants.DRIVEBASE_ENABLED) { bindDrivebaseControls(); } @@ -112,6 +117,10 @@ public Controls(Subsystems s) { if (Subsystems.SubsystemConstants.ARM_ENABLED) { bindArmControls(); } + + if (Subsystems.SubsystemConstants.LIMELIGHT_ENABLED) { + bindLimelightControls(); + } } public void bindDrivebaseControls() { @@ -199,4 +208,9 @@ public void bindLEDControls() { ledPurple.onTrue(new LEDPurpleCommand(s.ledSubsystem)); ledYellow.onTrue(new LEDYellowCommand(s.ledSubsystem)); } + + public void bindLimelightControls() { + getWithinDistanceTrigger.whileTrue( + new GetWithinDistanceCommand(s.limelightSubsystem, s.drivebaseSubsystem)); + } } diff --git a/src/main/java/frc/team2412/robot/Subsystems.java b/src/main/java/frc/team2412/robot/Subsystems.java index b50f1b82..d6cfaaa8 100644 --- a/src/main/java/frc/team2412/robot/Subsystems.java +++ b/src/main/java/frc/team2412/robot/Subsystems.java @@ -15,18 +15,20 @@ import frc.team2412.robot.subsystems.DrivebaseSubsystem; import frc.team2412.robot.subsystems.IntakeSubsystem; import frc.team2412.robot.subsystems.LEDSubsystem; +import frc.team2412.robot.subsystems.LimelightSubsystem; import frc.team2412.robot.subsystems.VisionSubsystem; public class Subsystems { public static class SubsystemConstants { public static final boolean IS_COMP = Robot.getInstance().isCompetition(); public static final boolean DRIVEBASE_ENABLED = true; - public static final boolean ARM_ENABLED = IS_COMP && true; - public static final boolean INTAKE_ENABLED = IS_COMP && true; + public static final boolean ARM_ENABLED = IS_COMP && false; + public static final boolean INTAKE_ENABLED = IS_COMP && false; public static final boolean VISION_ENABLED = true; public static final boolean LED_ENABLED = IS_COMP && true; public static final boolean ARM_LED_ENABLED = IS_COMP && true; - public static final boolean DRIVER_VIS_ENABLED = true; + public static final boolean DRIVER_VIS_ENABLED = false; + public static final boolean LIMELIGHT_ENABLED = true; } public DrivebaseSubsystem drivebaseSubsystem; @@ -35,6 +37,7 @@ public static class SubsystemConstants { public VisionSubsystem visionSubsystem; public LEDSubsystem ledSubsystem; public ArmLEDSubsystem armLedSubsystem; + public LimelightSubsystem limelightSubsystem; public SwerveDrivePoseEstimator poseEstimator; @@ -92,5 +95,8 @@ public Subsystems() { if (ARM_LED_ENABLED) { armLedSubsystem = new ArmLEDSubsystem(); } + if (LIMELIGHT_ENABLED) { + limelightSubsystem = new LimelightSubsystem(); + } } } diff --git a/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java b/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java index c8358469..3fae10ae 100644 --- a/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java +++ b/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java @@ -1,6 +1,7 @@ package frc.team2412.robot.commands.limelight; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.team2412.robot.subsystems.DrivebaseSubsystem; import frc.team2412.robot.subsystems.LimelightSubsystem; @@ -10,13 +11,22 @@ public class GetWithinDistanceCommand extends CommandBase { private LimelightSubsystem limelightSubsystem; private DrivebaseSubsystem drivebaseSubsystem; + private Command getToPositionCommand; + public GetWithinDistanceCommand( LimelightSubsystem limelightSubsystem, DrivebaseSubsystem drivebaseSubsystem) { this.limelightSubsystem = limelightSubsystem; this.drivebaseSubsystem = drivebaseSubsystem; + + Pose2d currentPose = drivebaseSubsystem.getPose(); + + // driver assist code + + getToPositionCommand = limelightSubsystem.getWithinDistance(currentPose, drivebaseSubsystem); } - public void execute() { + @Override + public void initialize() { // TOOD: use driver assist code // get drivebase pose -> feed into get pose method -> use driver assist code @@ -24,6 +34,20 @@ public void execute() { // driver assist code - limelightSubsystem.getWithinDistance(currentPose); + getToPositionCommand = limelightSubsystem.getWithinDistance(currentPose, drivebaseSubsystem); + getToPositionCommand.initialize(); + } + + @Override + public void execute() { + getToPositionCommand.execute(); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return limelightSubsystem.isWithinDistance(); } } diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index 98faa5bf..9ff8c105 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -1,14 +1,28 @@ package frc.team2412.robot.subsystems; +import com.pathplanner.lib.PathConstraints; +import com.pathplanner.lib.PathPlanner; +import com.pathplanner.lib.PathPlannerTrajectory; +import com.pathplanner.lib.PathPoint; +import com.pathplanner.lib.commands.PPSwerveControllerCommand; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class LimelightSubsystem extends SubsystemBase { // CONSTANTS + private static final PIDController TRANSLATION_PID = new PIDController(10.0, 0, 0); + private static final PIDController ROTATION_PID = new PIDController(8.0, 0, 0); + PathConstraints PATH_CONSTRAINTS = new PathConstraints(4.0, 2.0); + // meters? public static final double CAMERA_HEIGHT = 0.2; public static final double CAMERA_ANGLE_OFFSET = 0; @@ -20,13 +34,45 @@ public class LimelightSubsystem extends SubsystemBase { // MEMBERS NetworkTable networkTable; + String currentPoseString; + String targetPoseString; // network tables // CONSTRUCTOR ! public LimelightSubsystem() { + // logging + currentPoseString = ""; + targetPoseString = ""; + + ShuffleboardTab limelightTab = Shuffleboard.getTab("Limelight"); networkTable = NetworkTableInstance.getDefault().getTable("limelight"); + + limelightTab.addBoolean("hasTarget", this::hasTargets).withPosition(1, 0).withSize(1, 1); + limelightTab + .addDouble("Horizontal Offset", this::getHorizontalOffset) + .withPosition(2, 0) + .withSize(1, 1); + limelightTab + .addDouble("Vertical Offset", this::getVerticalOffset) + .withPosition(2, 0) + .withSize(1, 1); + + limelightTab + .addDouble("Target Distance ", this::getDistanceFromTarget) + .withPosition(4, 0) + .withSize(1, 1); + + limelightTab + .addString("Current Pose ", this::getCurrentPoseString) + .withPosition(3, 0) + .withSize(1, 1); + + limelightTab + .addString("Target Pose ", this::getTargetPoseString) + .withPosition(4, 0) + .withSize(1, 1); } // METHODS @@ -53,10 +99,81 @@ public Pose2d getTargetPose(Pose2d currentPose) { // math thing to get target pose using current pose - return currentPose; // replace later + Rotation2d currentHeading = currentPose.getRotation(); + Rotation2d targetHeading = new Rotation2d(currentHeading.getDegrees() + getHorizontalOffset()); + + double targetX = Math.sin(targetHeading.getDegrees()) * getDistanceFromTarget(); + double targetY = Math.cos(targetHeading.getDegrees()) * getDistanceFromTarget(); + + Pose2d targetPose = new Pose2d(targetX, targetY, targetHeading); + + currentPoseString = currentPose.toString(); + targetPoseString = targetPose.toString(); + + return targetPose; + } + + public String getCurrentPoseString() { + return currentPoseString; + } + + public String getTargetPoseString() { + return targetPoseString; + } + + public boolean isWithinDistance() { + return (getDistanceFromTarget() <= GOAL_DISTANCE_FROM_TARGET); + } + + public Command getWithinDistance(Pose2d currentPose, DrivebaseSubsystem drivebaseSubsystem) { + Pose2d targetPose; + if (hasTargets()) { + targetPose = getTargetPose(currentPose); + + } else { + targetPose = currentPose; + } + + System.out.println("current: " + drivebaseSubsystem.getPose()); + System.out.println("target: " + getTargetPose(drivebaseSubsystem.getPose())); + System.out.println("distance: " + getDistanceFromTarget()); + System.out.println("horizontal offset: " + getHorizontalOffset()); + System.out.println("vertical offset; " + getVerticalOffset()); + + PathPlannerTrajectory alignmentTraj = + PathPlanner.generatePath( + PATH_CONSTRAINTS, + new PathPoint( + currentPose.getTranslation(), + Rotation2d.fromRadians( + Math.atan2( + targetPose.getY() - currentPose.getY(), + targetPose.getX() - currentPose.getX())), + currentPose.getRotation(), + drivebaseSubsystem.getVelocity()), + new PathPoint( + targetPose.getTranslation(), + Rotation2d.fromDegrees(180), + targetPose.getRotation())); + + Command moveCommand = + new PPSwerveControllerCommand( + alignmentTraj, + drivebaseSubsystem::getPose, + DrivebaseSubsystem.kinematics, + TRANSLATION_PID, + TRANSLATION_PID, + ROTATION_PID, + drivebaseSubsystem::drive, + true, + drivebaseSubsystem); + + return moveCommand; } - public void getWithinDistance(Pose2d currentPose) { - getTargetPose(currentPose); + @Override + public void periodic() { + System.out.println("has target " + hasTargets()); + System.out.println("distance " + getDistanceFromTarget()); } } From 71caff55c4071c0abf3dafa9adb9fcd915179a19 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Wed, 10 Jan 2024 11:09:18 -0800 Subject: [PATCH 3/4] cleanup --- .../limelight/GetWithinDistanceCommand.java | 16 +++------ .../robot/subsystems/LimelightSubsystem.java | 34 +++++++++---------- 2 files changed, 20 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java b/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java index 3fae10ae..bf59ea69 100644 --- a/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java +++ b/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java @@ -18,23 +18,14 @@ public GetWithinDistanceCommand( this.limelightSubsystem = limelightSubsystem; this.drivebaseSubsystem = drivebaseSubsystem; - Pose2d currentPose = drivebaseSubsystem.getPose(); - - // driver assist code + // find target -> get current pose -> use current pose and target to calculate target pose -> move towards target pose + Pose2d currentPose = drivebaseSubsystem.getPose(); getToPositionCommand = limelightSubsystem.getWithinDistance(currentPose, drivebaseSubsystem); } @Override public void initialize() { - // TOOD: use driver assist code - // get drivebase pose -> feed into get pose method -> use driver assist code - - Pose2d currentPose = drivebaseSubsystem.getPose(); - - // driver assist code - - getToPositionCommand = limelightSubsystem.getWithinDistance(currentPose, drivebaseSubsystem); getToPositionCommand.initialize(); } @@ -44,7 +35,8 @@ public void execute() { } @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + } @Override public boolean isFinished() { diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index 9ff8c105..7f6e067b 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -45,14 +45,14 @@ public LimelightSubsystem() { // logging currentPoseString = ""; targetPoseString = ""; - - ShuffleboardTab limelightTab = Shuffleboard.getTab("Limelight"); + networkTable = NetworkTableInstance.getDefault().getTable("limelight"); + ShuffleboardTab limelightTab = Shuffleboard.getTab("Limelight"); - limelightTab.addBoolean("hasTarget", this::hasTargets).withPosition(1, 0).withSize(1, 1); + limelightTab.addBoolean("hasTarget", this::hasTargets).withPosition(0, 0).withSize(1, 1); limelightTab .addDouble("Horizontal Offset", this::getHorizontalOffset) - .withPosition(2, 0) + .withPosition(1, 0) .withSize(1, 1); limelightTab .addDouble("Vertical Offset", this::getVerticalOffset) @@ -61,18 +61,18 @@ public LimelightSubsystem() { limelightTab .addDouble("Target Distance ", this::getDistanceFromTarget) - .withPosition(4, 0) + .withPosition(3, 0) .withSize(1, 1); limelightTab .addString("Current Pose ", this::getCurrentPoseString) - .withPosition(3, 0) - .withSize(1, 1); + .withPosition(0, 1) + .withSize(3, 1); limelightTab .addString("Target Pose ", this::getTargetPoseString) - .withPosition(4, 0) - .withSize(1, 1); + .withPosition(0, 2) + .withSize(3, 1); } // METHODS @@ -99,11 +99,14 @@ public Pose2d getTargetPose(Pose2d currentPose) { // math thing to get target pose using current pose + // FIXME: figure out why targetPose returns infinity + Rotation2d currentHeading = currentPose.getRotation(); Rotation2d targetHeading = new Rotation2d(currentHeading.getDegrees() + getHorizontalOffset()); + double targetDistance = getDistanceFromTarget() - GOAL_DISTANCE_FROM_TARGET; - double targetX = Math.sin(targetHeading.getDegrees()) * getDistanceFromTarget(); - double targetY = Math.cos(targetHeading.getDegrees()) * getDistanceFromTarget(); + double targetX = Math.sin(targetHeading.getDegrees()) * targetDistance; + double targetY = Math.cos(targetHeading.getDegrees()) * targetDistance; Pose2d targetPose = new Pose2d(targetX, targetY, targetHeading); @@ -134,11 +137,7 @@ public Command getWithinDistance(Pose2d currentPose, DrivebaseSubsystem drivebas targetPose = currentPose; } - System.out.println("current: " + drivebaseSubsystem.getPose()); - System.out.println("target: " + getTargetPose(drivebaseSubsystem.getPose())); - System.out.println("distance: " + getDistanceFromTarget()); - System.out.println("horizontal offset: " + getHorizontalOffset()); - System.out.println("vertical offset; " + getVerticalOffset()); + // create path PathPlannerTrajectory alignmentTraj = PathPlanner.generatePath( @@ -156,6 +155,7 @@ public Command getWithinDistance(Pose2d currentPose, DrivebaseSubsystem drivebas Rotation2d.fromDegrees(180), targetPose.getRotation())); + // make command out of path Command moveCommand = new PPSwerveControllerCommand( alignmentTraj, @@ -173,7 +173,5 @@ public Command getWithinDistance(Pose2d currentPose, DrivebaseSubsystem drivebas @Override public void periodic() { - System.out.println("has target " + hasTargets()); - System.out.println("distance " + getDistanceFromTarget()); } } From 7d671efa1ee91f42641f821d36fae7a76fb1e9a6 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Wed, 10 Jan 2024 11:13:17 -0800 Subject: [PATCH 4/4] spotless --- .../robot/commands/limelight/GetWithinDistanceCommand.java | 6 +++--- .../frc/team2412/robot/subsystems/LimelightSubsystem.java | 5 ++--- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java b/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java index bf59ea69..6a85f36d 100644 --- a/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java +++ b/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java @@ -18,7 +18,8 @@ public GetWithinDistanceCommand( this.limelightSubsystem = limelightSubsystem; this.drivebaseSubsystem = drivebaseSubsystem; - // find target -> get current pose -> use current pose and target to calculate target pose -> move towards target pose + // find target -> get current pose -> use current pose and target to calculate target pose -> + // move towards target pose Pose2d currentPose = drivebaseSubsystem.getPose(); getToPositionCommand = limelightSubsystem.getWithinDistance(currentPose, drivebaseSubsystem); @@ -35,8 +36,7 @@ public void execute() { } @Override - public void end(boolean interrupted) { - } + public void end(boolean interrupted) {} @Override public boolean isFinished() { diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index 7f6e067b..24bf6925 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -45,7 +45,7 @@ public LimelightSubsystem() { // logging currentPoseString = ""; targetPoseString = ""; - + networkTable = NetworkTableInstance.getDefault().getTable("limelight"); ShuffleboardTab limelightTab = Shuffleboard.getTab("Limelight"); @@ -172,6 +172,5 @@ public Command getWithinDistance(Pose2d currentPose, DrivebaseSubsystem drivebas } @Override - public void periodic() { - } + public void periodic() {} }