lime stuff works

src/main/java/frc4388/robot/Constants.java

@@ -159,6 +159,6 @@ public final class Constants {
     public static final double HIGH_TAPE_HEIGHT = 44.0;
     
     // public static final double MID_TARGET_HEIGHT = 34.0;
-    public static final double MID_TAPE_HEIGHT = 32.0;
+    public static final double MID_TAPE_HEIGHT = 24.0;
   }
 }

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

@@ -20,7 +20,7 @@ import frc4388.robot.subsystems.Claw;
 import frc4388.robot.subsystems.Limelight;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.commands.JoystickRecorder;
-import frc4388.robot.commands.PivotCommand;
+import frc4388.robot.commands.LimeAlign;
 import frc4388.robot.commands.PlaybackChooser;
 import frc4388.utility.controller.DeadbandedXboxController;
 import frc4388.utility.controller.XboxController; 
@@ -144,10 +144,15 @@ public class RobotContainer {
 
         
 
-        // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
-        //     .onTrue(new InstantCommand(() -> m_limeLight.toggleLEDs(), m_limeLight));
+            .onTrue(new InstantCommand(() -> m_limeLight.toggleLEDs(), m_limeLight));
+
         
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
+        
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
+            .whileTrue(new LimeAlign(m_robotSwerveDrive, m_limeLight));
+        
+            new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotClaw.toggle()));
         
         new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)

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

@@ -4,20 +4,20 @@
 
 package frc4388.robot.commands;
 
+import org.photonvision.targeting.PhotonTrackedTarget;
+
 import edu.wpi.first.math.geometry.Translation2d;
 import frc4388.robot.Constants.VisionConstants;
 import frc4388.robot.subsystems.Limelight;
 import frc4388.robot.subsystems.SwerveDrive;
-import frc4388.utility.AbhiIsADumbass;
 
 public class LimeAlign extends PelvicInflammatoryDisease {
 
   SwerveDrive drive;
   Limelight lime;
 
-
   public LimeAlign(SwerveDrive drive, Limelight lime) {
-    super(0.1, 0.0, 0.0, 0.0, 10);
+    super(0.7, 0.1, 0.0, 0.0, 0);
 
     this.drive = drive;
     this.lime = lime;
@@ -27,16 +27,24 @@ public class LimeAlign extends PelvicInflammatoryDisease {
 
   @Override
   public double getError() {
+    double err = 0.0;
+
     try {
-      return lime.getTargetPoints().get(0).x - (VisionConstants.LIME_HIXELS / 2);
+      err = lime.getFirstTargetPoint().getYaw() / (VisionConstants.H_FOV / 2); 
-    } catch (AbhiIsADumbass abhiIsADumbass) {
+    } catch (NullPointerException ex) {}
-      abhiIsADumbass.printStackTrace();
+    
-      return 0;
+    return err;
-    }
   }
 
   @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

@@ -23,13 +23,13 @@ public class Limelight extends SubsystemBase {
   private PhotonCamera cam;
 
   private boolean lightOn;
-  private boolean isConnected = false;
 
   /** Creates a new Limelight. */
   public Limelight() {
     cam = new PhotonCamera(VisionConstants.NAME);
-    isConnected = cam.isConnected();
     cam.setDriverMode(false);
+    
+    setToLimePipeline();
   }
 
   public void setLEDs(boolean on) {
@@ -46,16 +46,24 @@ public class Limelight extends SubsystemBase {
     cam.setDriverMode(driverMode);
   }
 
-  public ArrayList<Point> getTargetPoints() throws AbhiIsADumbass {
+  public void setToLimePipeline() {
+    cam.setPipelineIndex(1);
+  }
+
+  public void setToAprilPipeline() {
+    cam.setPipelineIndex(0);
+  }
+
+  public ArrayList<Point> getTargetPoints() {
     if (!cam.isConnected()) return null;
 
     PhotonPipelineResult result = cam.getLatestResult();
 
-    if (!result.hasTargets()) throw new AbhiIsADumbass();
+    if (!result.hasTargets()) return null;
 
     ArrayList<Point> points = new ArrayList<>(2);
 
-    for(PhotonTrackedTarget target : result.getTargets()){
+    for(PhotonTrackedTarget target : result.getTargets()) {
       List<TargetCorner> corners = target.getDetectedCorners();
   
       double sumX = 0.0;
@@ -77,38 +85,50 @@ public class Limelight extends SubsystemBase {
     return points;
   }
 
+  public PhotonTrackedTarget getFirstTargetPoint() {
+    if (!cam.isConnected()) return null;
+
+    PhotonPipelineResult result = cam.getLatestResult();
+
+    if (!result.hasTargets()) return null;
+
+    return result.getBestTarget();
+  }
+
   private double getPointAngle(Point point) {
     return (VisionConstants.LIME_VIXELS - point.y) * (VisionConstants.V_FOV / VisionConstants.LIME_VIXELS);
   }
 
-  public double getHorizontalDistanceToTarget(boolean high) throws AbhiIsADumbass {
+  public double getHorizontalDistanceToTarget(boolean high) {
     ArrayList<Point> 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);
 
-    Point tapePoint = targetPoints.get(0);//high ? highPoint : midPoint;
+    PhotonTrackedTarget tapePoint = getFirstTargetPoint();//high ? highPoint : midPoint;
     double tapeHeight = VisionConstants.MID_TAPE_HEIGHT;//high ? VisionConstants.HIGH_TAPE_HEIGHT : VisionConstants.MID_TAPE_HEIGHT;
 
-    double theta = VisionConstants.LIME_ANGLE + getPointAngle(tapePoint);
+    double theta = 35.0 + tapePoint.getPitch();
 
     double effectiveTapeHeight = tapeHeight - VisionConstants.LIME_HEIGHT;
 
-    double distanceToTape = effectiveTapeHeight / Math.sin(Math.toRadians(theta));
+    double horizontalDistanceToTarget = effectiveTapeHeight / Math.tan(Math.toRadians(theta));
-
-    double horizontalDistanceToTarget = Math.sqrt(Math.pow(distanceToTape, 2) - Math.pow(effectiveTapeHeight, 2));
 
     return horizontalDistanceToTarget;
   }
 
+  int ctr = 0;
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
-    try {
+    
-      System.out.println(getHorizontalDistanceToTarget(false));
+    if (ctr % 50 == 0) {
-    } catch(AbhiIsADumbass abhiIsADumbass) {
+      SmartDashboard.putNumber("Horizontal Distance", getHorizontalDistanceToTarget(false));
-      abhiIsADumbass.printStackTrace();
+    }
-    } 
+
+    ctr++;
+
   }
 }