aprilrotalign works

src/main/java/frc4388/robot/RobotContainer.java

@@ -7,7 +7,6 @@
 
 package frc4388.robot;
 
-import java.lang.StackWalker.Option;
 import java.util.function.Consumer;
 
 import edu.wpi.first.math.geometry.Translation2d;
@@ -29,7 +28,7 @@ import frc4388.robot.commands.Arm.PivotCommand;
 import frc4388.robot.commands.Arm.TeleCommand;
 import frc4388.robot.commands.Autos.AutoBalance;
 import frc4388.robot.commands.Autos.PlaybackChooser;
-import frc4388.robot.commands.Placement.DriveToLimeDistance;
+import frc4388.robot.commands.Placement.AprilRotAlign;
 import frc4388.robot.commands.Placement.LimeAlign;
 import frc4388.robot.commands.Swerve.RotateToAngle;
 import frc4388.utility.controller.DeadbandedXboxController;
@@ -263,8 +262,10 @@ public class RobotContainer {
             .onFalse(interruptCommand.asProxy());
 
         // toggle claw
+        // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final
+        //     .onTrue(toggleClaw.asProxy());
         new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final
-            .onTrue(toggleClaw.asProxy());
+            .whileTrue(new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight));
 
         // kill soft limits
         new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) // final

src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java

@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.Placement;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import frc4388.robot.commands.PelvicInflammatoryDisease;
+import frc4388.robot.subsystems.Limelight;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class AprilRotAlign extends PelvicInflammatoryDisease {
+  
+  SwerveDrive drive;
+  Limelight lime;
+  
+  /** Creates a new AprilRotAlign. */
+  public AprilRotAlign(SwerveDrive drive, Limelight lime) {
+    super(0.1, 0.2, 0.0, 0.0, 0.0);
+  
+    this.drive = drive;
+    this.lime = lime;
+
+    addRequirements(drive, lime);
+  }
+
+  @Override
+  public double getError() {
+    double err = 0.0;
+
+    try {
+      err = lime.getAprilSkew();
+    } catch (NullPointerException ex) {}
+    
+    return err;
+  }
+
+  @Override
+  public void runWithOutput(double output) {
+    drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(-output, 0.0), true);
+  }
+}

src/main/java/frc4388/robot/commands/Placement/LimeAlign.java

@@ -43,13 +43,6 @@ public class LimeAlign extends PelvicInflammatoryDisease {
 
   @Override
   public void runWithOutput(double output) {
-
-    // if (output > 0) {
-    //   output += 0.6;
-    // } else if (output < 0) {
-    //   output -= 0.6;
-    // }
-
     drive.driveWithInput(new Translation2d(output, 0.0), new Translation2d(0.0, 0.0), true);
   }
 }

src/main/java/frc4388/robot/subsystems/Limelight.java

@@ -5,11 +5,13 @@
 package frc4388.robot.subsystems;
 
 import java.util.ArrayList;
+import java.util.List;
 
 import org.photonvision.PhotonCamera;
 import org.photonvision.common.hardware.VisionLEDMode;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
+import org.photonvision.targeting.TargetCorner;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -50,7 +52,6 @@ public class Limelight extends SubsystemBase {
     setLEDs(false);
   }
 
-  // ! might need to find midpoint instead of entire target
   public PhotonTrackedTarget getAprilPoint() {
     if (!cam.isConnected()) return null;
 
@@ -61,6 +62,51 @@ public class Limelight extends SubsystemBase {
     return result.getBestTarget();
   }
 
+  private List<TargetCorner> getAprilCorners() {
+    if (!cam.isConnected()) return null;
+
+    PhotonPipelineResult result = cam.getLatestResult();
+
+    if (!result.hasTargets()) return null;
+
+    return result.getBestTarget().getDetectedCorners();
+  }
+
+  public double getAprilSkew() {
+    List<TargetCorner> corners = getAprilCorners();
+    ArrayList<TargetCorner> bottomSide = getAprilBottomSide(corners);
+
+    if (bottomSide == null) return 0;
+
+    TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1);
+    TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0);
+
+    return bottomLeft.y - bottomRight.y;
+  }
+
+  private ArrayList<TargetCorner> getAprilBottomSide(List<TargetCorner> box) {
+    if (box == null) return null;
+
+    ArrayList<TargetCorner> bottomSide = new ArrayList<>();
+
+    TargetCorner l1 = new TargetCorner(-1, -1);
+    TargetCorner l2 = new TargetCorner(-1, -1);
+    
+    for (TargetCorner c : box) {
+      if (c.y > l1.y) l1 = c;
+    }
+
+    for (TargetCorner c : box) {
+      if (c.y == l1.y) continue;
+      if (c.y > l2.y) l2 = c;
+    }
+
+    bottomSide.add(l1);
+    bottomSide.add(l2);
+
+    return bottomSide;
+  }
+
   public double getDistanceToApril() {    
     PhotonTrackedTarget aprilPoint = getAprilPoint();
     if (aprilPoint == null) return -1;
@@ -102,17 +148,8 @@ public class Limelight extends SubsystemBase {
     return distanceToTape;
   }
 
-  int ctr = 0;
-
   @Override
   public void periodic() {
-    // This method will be called once per scheduler run
-    
-    if (ctr % 50 == 0) {
-      SmartDashboard.putNumber("Horizontal Distance", getDistanceToTape());
-    }
-
-    ctr++;
-
+    SmartDashboard.putNumber("April Skew", getAprilSkew());
   }
 }