updated limealign and drivetolimedistance

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
   }
 }

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 LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw()),
-            new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 30)
+            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 SequentialCommandGroup alignToShelf = new SequentialCommandGroup(
-    private ConditionalCommand alignToShelf = new ConditionalCommand(
+        new RotateToAngle(m_robotSwerveDrive, 0.0),
-        new SequentialCommandGroup(
+        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)),
+    ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false));
-        emptyCommand.asProxy(),
-        () -> true
-    );
 
     public SequentialCommandGroup place = null;
 

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

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;

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<Point> getTargetPoints() {
+  public PhotonTrackedTarget getAprilPoint() {
-    if (!cam.isConnected()) return null;
+    setToAprilPipeline();
 
-    PhotonPipelineResult result = cam.getLatestResult();
-
-    if (!result.hasTargets()) return null;
-
-    ArrayList<Point> points = new ArrayList<>(2);
-
-    for(PhotonTrackedTarget target : result.getTargets()) {
-      List<TargetCorner> 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) {
+  public double getDistanceToTape() {
-    ArrayList<Point> targetPoints = getTargetPoints();
+    PhotonTrackedTarget tapePoint = getLowestTape();
-    if (targetPoints == null) return -1;
+    if (tapePoint == 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;
 
+    double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT;
     double theta = 35.0 + tapePoint.getPitch();
 
-    double effectiveTapeHeight = tapeHeight - VisionConstants.LIME_HEIGHT;
+    double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta));
-
+    return distanceToTape;
-    double horizontalDistanceToTarget = effectiveTapeHeight / Math.tan(Math.toRadians(theta));
-
-    return horizontalDistanceToTarget;
   }
 
   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++;