From 330d2a781740655261526bb3bdf49b8bbd56ec54 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 16 Mar 2023 23:08:08 -0600 Subject: [PATCH] updated limealign and drivetolimedistance --- src/main/java/frc4388/robot/Constants.java | 2 + .../java/frc4388/robot/RobotContainer.java | 19 ++--- .../Placement/DriveToLimeDistance.java | 8 +- .../robot/commands/Placement/LimeAlign.java | 11 ++- .../frc4388/robot/subsystems/Limelight.java | 76 +++++++------------ 5 files changed, 50 insertions(+), 66 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 9fa45e3..fc518b1 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -168,5 +168,7 @@ public final class Constants { // public static final double MID_TARGET_HEIGHT = 34.0; public static final double MID_TAPE_HEIGHT = 24.0; + + public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 64bb18e..50fcd85 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -92,21 +92,18 @@ public class RobotContainer { private ConditionalCommand alignToPole = new ConditionalCommand( new SequentialCommandGroup( new RotateToAngle(m_robotSwerveDrive, 0.0), - new LimeAlign(m_robotSwerveDrive, m_robotLimeLight), - new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 30) + new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw()), + new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 30, () -> m_robotLimeLight.getDistanceToTape()) ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true)), emptyCommand.asProxy(), - () -> m_robotLimeLight.numTargets() <= 2 + () -> m_robotLimeLight.getNumTapes() <= 2 ); - // TODO: make - private ConditionalCommand alignToShelf = new ConditionalCommand( - new SequentialCommandGroup( - - ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false)), - emptyCommand.asProxy(), - () -> true - ); + private SequentialCommandGroup alignToShelf = new SequentialCommandGroup( + new RotateToAngle(m_robotSwerveDrive, 0.0), + new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw()), + new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 30, () -> m_robotLimeLight.getDistanceToApril()) // TODO: find distance + ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false)); public SequentialCommandGroup place = null; diff --git a/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java b/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java index b74a395..ac49334 100644 --- a/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java +++ b/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java @@ -4,6 +4,8 @@ package frc4388.robot.commands.Placement; +import java.util.function.DoubleSupplier; + import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.RobotContainer; @@ -17,21 +19,23 @@ public class DriveToLimeDistance extends PelvicInflammatoryDisease { Limelight lime; double targetDistance; + DoubleSupplier ds; /** Creates a new DriveToLimeDistance. */ - public DriveToLimeDistance(SwerveDrive drive, Limelight lime, double targetDistance) { + public DriveToLimeDistance(SwerveDrive drive, Limelight lime, double targetDistance, DoubleSupplier ds) { super(0.2, 0.0, 0.0, 0.0, 1); this.drive = drive; this.lime = lime; this.targetDistance = targetDistance; + this.ds = ds; addRequirements(drive, lime); } @Override public double getError() { - return lime.getHorizontalDistanceToTarget(false) - targetDistance; + return ds.getAsDouble() - targetDistance; } @Override diff --git a/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java b/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java index e707459..1079c6d 100644 --- a/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java +++ b/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java @@ -4,6 +4,8 @@ package frc4388.robot.commands.Placement; +import java.util.function.DoubleSupplier; + import edu.wpi.first.math.geometry.Translation2d; import frc4388.robot.Constants.VisionConstants; import frc4388.robot.commands.PelvicInflammatoryDisease; @@ -15,11 +17,14 @@ public class LimeAlign extends PelvicInflammatoryDisease { SwerveDrive drive; Limelight lime; - public LimeAlign(SwerveDrive drive, Limelight lime) { + DoubleSupplier ds; + + public LimeAlign(SwerveDrive drive, Limelight lime, DoubleSupplier ds) { super(0.7, 0.4, 0.0, 0.0, 0.04); this.drive = drive; this.lime = lime; + this.ds = ds; addRequirements(drive, lime); } @@ -27,14 +32,14 @@ public class LimeAlign extends PelvicInflammatoryDisease { @Override public double getError() { - if (lime.numTargets() > 2) { + if (lime.getNumTapes() > 2) { return 0.0; } double err = 0.0; try { - err = lime.getLowestTargetPoint().getYaw() / (VisionConstants.H_FOV / 2); + err = ds.getAsDouble() / (VisionConstants.H_FOV / 2); } catch (NullPointerException ex) {} return err; diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index c53a62b..8fbe5a9 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -28,8 +28,6 @@ public class Limelight extends SubsystemBase { public Limelight() { cam = new PhotonCamera(VisionConstants.NAME); cam.setDriverMode(false); - - setToLimePipeline(); } public void setLEDs(boolean on) { @@ -54,38 +52,9 @@ public class Limelight extends SubsystemBase { cam.setPipelineIndex(0); } - public ArrayList getTargetPoints() { - if (!cam.isConnected()) return null; + public PhotonTrackedTarget getAprilPoint() { + setToAprilPipeline(); - PhotonPipelineResult result = cam.getLatestResult(); - - if (!result.hasTargets()) return null; - - ArrayList points = new ArrayList<>(2); - - for(PhotonTrackedTarget target : result.getTargets()) { - List corners = target.getDetectedCorners(); - - double sumX = 0.0; - double sumY = 0.0; - double mx = 0.0; - double my = 0.0; - - for (TargetCorner c : corners) { - sumX += c.x; - sumY += c.y; - } - - mx = sumX / 4.0; - my = sumY / 4.0; - - points.add(new Point(mx, my)); - } - - return points; - } - - public PhotonTrackedTarget getFirstTargetPoint() { if (!cam.isConnected()) return null; PhotonPipelineResult result = cam.getLatestResult(); @@ -95,7 +64,20 @@ public class Limelight extends SubsystemBase { return result.getBestTarget(); } - public PhotonTrackedTarget getLowestTargetPoint() { + public double getDistanceToApril() { + PhotonTrackedTarget aprilPoint = getAprilPoint(); + if (aprilPoint == null) return -1; + + double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT; + double theta = 35.0 + aprilPoint.getPitch(); + + double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta)); + return distanceToApril; + } + + public PhotonTrackedTarget getLowestTape() { + setToLimePipeline(); + if (!cam.isConnected()) return null; PhotonPipelineResult result = cam.getLatestResult(); @@ -114,29 +96,23 @@ public class Limelight extends SubsystemBase { return lowest; } - public int numTargets() { + public int getNumTapes() { + setToLimePipeline(); + PhotonPipelineResult result = cam.getLatestResult(); return result.getTargets().size(); } - public double getHorizontalDistanceToTarget(boolean high) { - ArrayList targetPoints = getTargetPoints(); - if (targetPoints == null) return -1; - - // Point highPoint = targetPoints.get(0).y <= targetPoints.get(1).y ? targetPoints.get(0) : targetPoints.get(1); - // Point midPoint = targetPoints.get(0).y >= targetPoints.get(1).y ? targetPoints.get(0) : targetPoints.get(1); - - PhotonTrackedTarget tapePoint = getFirstTargetPoint();//high ? highPoint : midPoint; - double tapeHeight = VisionConstants.MID_TAPE_HEIGHT;//high ? VisionConstants.HIGH_TAPE_HEIGHT : VisionConstants.MID_TAPE_HEIGHT; + public double getDistanceToTape() { + PhotonTrackedTarget tapePoint = getLowestTape(); + if (tapePoint == null) return -1; + double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT; double theta = 35.0 + tapePoint.getPitch(); - double effectiveTapeHeight = tapeHeight - VisionConstants.LIME_HEIGHT; - - double horizontalDistanceToTarget = effectiveTapeHeight / Math.tan(Math.toRadians(theta)); - - return horizontalDistanceToTarget; + double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta)); + return distanceToTape; } int ctr = 0; @@ -146,7 +122,7 @@ public class Limelight extends SubsystemBase { // This method will be called once per scheduler run if (ctr % 50 == 0) { - SmartDashboard.putNumber("Horizontal Distance", getHorizontalDistanceToTarget(false)); + SmartDashboard.putNumber("Horizontal Distance", getDistanceToTape()); } ctr++;