From 6b2ca9e4332fb74dc84ab3fadf6b97d9bb69e758 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 16 Mar 2023 16:25:26 -0600 Subject: [PATCH] lime stuff works --- src/main/java/frc4388/robot/Constants.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 13 +++-- .../frc4388/robot/commands/LimeAlign.java | 24 ++++++--- .../frc4388/robot/subsystems/Limelight.java | 52 +++++++++++++------ 4 files changed, 62 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 39b6c3f..74db48f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/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; } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f7e61c7..ebe71ec 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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) - // .onTrue(new InstantCommand(() -> m_limeLight.toggleLEDs(), m_limeLight)); + new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) + .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) diff --git a/src/main/java/frc4388/robot/commands/LimeAlign.java b/src/main/java/frc4388/robot/commands/LimeAlign.java index 289a56c..533b6f0 100644 --- a/src/main/java/frc4388/robot/commands/LimeAlign.java +++ b/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); - } catch (AbhiIsADumbass abhiIsADumbass) { - abhiIsADumbass.printStackTrace(); - return 0; - } + err = lime.getFirstTargetPoint().getYaw() / (VisionConstants.H_FOV / 2); + } catch (NullPointerException ex) {} + + 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); } } diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index 18250a2..c5f3c50 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/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 getTargetPoints() throws AbhiIsADumbass { + public void setToLimePipeline() { + cam.setPipelineIndex(1); + } + + public void setToAprilPipeline() { + cam.setPipelineIndex(0); + } + + public ArrayList getTargetPoints() { if (!cam.isConnected()) return null; PhotonPipelineResult result = cam.getLatestResult(); - if (!result.hasTargets()) throw new AbhiIsADumbass(); + if (!result.hasTargets()) return null; ArrayList points = new ArrayList<>(2); - for(PhotonTrackedTarget target : result.getTargets()){ + for(PhotonTrackedTarget target : result.getTargets()) { List 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 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 = Math.sqrt(Math.pow(distanceToTape, 2) - Math.pow(effectiveTapeHeight, 2)); + double horizontalDistanceToTarget = effectiveTapeHeight / Math.tan(Math.toRadians(theta)); return horizontalDistanceToTarget; } + int ctr = 0; + @Override public void periodic() { // This method will be called once per scheduler run - try { - System.out.println(getHorizontalDistanceToTarget(false)); - } catch(AbhiIsADumbass abhiIsADumbass) { - abhiIsADumbass.printStackTrace(); - } + + if (ctr % 50 == 0) { + SmartDashboard.putNumber("Horizontal Distance", getHorizontalDistanceToTarget(false)); + } + + ctr++; + } }