From e0f15a8e97e57707a68c401858163a16f8b5f3c8 Mon Sep 17 00:00:00 2001 From: Aarav Date: Wed, 15 Mar 2023 13:50:35 -0600 Subject: [PATCH 1/7] lime stuff --- src/main/java/frc4388/robot/Constants.java | 15 ++++++ .../frc4388/robot/subsystems/Limelight.java | 53 +++++++++++++++++++ .../java/frc4388/utility/AbhiIsADumbass.java | 43 +++++++++++++++ vendordeps/photonlib.json | 41 ++++++++++++++ 4 files changed, 152 insertions(+) create mode 100644 src/main/java/frc4388/robot/subsystems/Limelight.java create mode 100644 src/main/java/frc4388/utility/AbhiIsADumbass.java create mode 100644 vendordeps/photonlib.json diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 83ac014..9953934 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -142,4 +142,19 @@ public final class Constants { public static final double RIGHT_AXIS_DEADBAND = 0.6; public static final boolean SKEW_STICKS = true; // ! this might have to actually be false, merge conflicts are confusing } + + public static final class VisionConstants { + public static final String NAME = "Aarav is poopy"; + + public static final int LIME_FOV = -1; + + public static final double LIME_HEIGHT = -1.0; + public static final double LIME_ANGLE = -1.0; + + public static final double HIGH_TARGET_HEIGHT = -1.0; + public static final double HIGHT_TAPE_HEIGHT = -1.0; + + public static final double MID_TARGET_HEIGHT = -1.0; + public static final double MID_TAPE_HEIGHT = -1.0; + } } diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java new file mode 100644 index 0000000..a6d951a --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -0,0 +1,53 @@ +// 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.subsystems; + +import java.util.ArrayList; +import java.util.List; + +import org.opencv.core.Point; +import org.photonvision.PhotonCamera; +import org.photonvision.common.hardware.VisionLEDMode; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.TargetCorner; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.VisionConstants; +import frc4388.utility.AbhiIsADumbass; + +public class Limelight extends SubsystemBase { + private PhotonCamera cam; + /** Creates a new LImelight. */ + public Limelight() { + cam = new PhotonCamera(VisionConstants.NAME); + } + + public void setLEDs(boolean on) { + cam.setLED(on ? VisionLEDMode.kOn : VisionLEDMode.kOff); + } + + public void setDriverMode(boolean driverMode) { + cam.setDriverMode(driverMode); + } + + public Point getTargetPoints() throws AbhiIsADumbass { + PhotonPipelineResult result = cam.getLatestResult(); + + if (!result.hasTargets()) throw new AbhiIsADumbass(); + + Point point = new Point(); + List corners = result.getTargets().get(0).getDetectedCorners(); + + double x1, x2, y1, y2; + double mx, my; + + return new Point(mx, my); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc4388/utility/AbhiIsADumbass.java b/src/main/java/frc4388/utility/AbhiIsADumbass.java new file mode 100644 index 0000000..14a9515 --- /dev/null +++ b/src/main/java/frc4388/utility/AbhiIsADumbass.java @@ -0,0 +1,43 @@ +// 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.utility; + +/** Exception that occurs if the limelight can't see enough points + * @author Abhi Sachi + * @author Aarav Shah + */ +public class AbhiIsADumbass extends Exception { + /** + * Creates new AbhiIsADumbass with error text 'null' + */ + public AbhiIsADumbass() { + super("Unable to see sufficient vision points (Abhi is a dumbass)"); + } + + /** Creates new AbhiIsADumbass with error text message + * + * @param message Error text message + */ + public AbhiIsADumbass(String message) { + super(message); + } + + /** Creates new AbhiIsADumbass with error text message and detailed stack trace + * + * @param message Error text message + * @param cause Root cause of error + */ + public AbhiIsADumbass(String message, Throwable cause) { + super(message, cause); + } + + /** Creates new AbhiIsADumbass with error text 'null' and detailed stack trace + * + * @param cause Root cause of error + */ + public AbhiIsADumbass(Throwable cause) { + super("Unable to see sufficient vision points (Abhi is a dumbass)", cause); + } +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..dad3105 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,41 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2023.4.2", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-cpp", + "version": "v2023.4.2", + "libName": "Photon", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-java", + "version": "v2023.4.2" + }, + { + "groupId": "org.photonvision", + "artifactId": "PhotonTargeting-java", + "version": "v2023.4.2" + } + ] +} \ No newline at end of file From 91ef73a441c10552a8e1ed7de85624a5b2a30ee9 Mon Sep 17 00:00:00 2001 From: Abhishrek05 Date: Wed, 15 Mar 2023 14:50:30 -0600 Subject: [PATCH 2/7] gud --- src/main/java/frc4388/robot/Constants.java | 8 ++- .../frc4388/robot/subsystems/Limelight.java | 55 +++++++++++++++++-- 2 files changed, 55 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 9953934..d58160e 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -146,13 +146,17 @@ public final class Constants { public static final class VisionConstants { public static final String NAME = "Aarav is poopy"; - public static final int LIME_FOV = -1; + public static final int LIME_HIXELS = 640; + public static final int LIME_VIXELS = 480; + + public static final double H_FOV = 59.6; + public static final double V_FOV = 45.7; public static final double LIME_HEIGHT = -1.0; public static final double LIME_ANGLE = -1.0; public static final double HIGH_TARGET_HEIGHT = -1.0; - public static final double HIGHT_TAPE_HEIGHT = -1.0; + public static final double HIGH_TAPE_HEIGHT = -1.0; public static final double MID_TARGET_HEIGHT = -1.0; public static final double MID_TAPE_HEIGHT = -1.0; diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index a6d951a..9c91afb 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -11,8 +11,10 @@ import org.opencv.core.Point; 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.math.Pair; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.VisionConstants; import frc4388.utility.AbhiIsADumbass; @@ -32,18 +34,59 @@ public class Limelight extends SubsystemBase { cam.setDriverMode(driverMode); } - public Point getTargetPoints() throws AbhiIsADumbass { + public ArrayList getTargetPoints() throws AbhiIsADumbass { PhotonPipelineResult result = cam.getLatestResult(); if (!result.hasTargets()) throw new AbhiIsADumbass(); - Point point = new Point(); - List corners = result.getTargets().get(0).getDetectedCorners(); + ArrayList points = new ArrayList<>(2); - double x1, x2, y1, y2; - double mx, my; + 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; - return new Point(mx, my); + points.add(new Point(mx, my)); + } + + return points; + } + + private double getPointAngle(Point point) { + return (VisionConstants.LIME_VIXELS - point.y) * (VisionConstants.V_FOV / VisionConstants.LIME_VIXELS); + } + + public double getDistanceToTarget(boolean high) throws AbhiIsADumbass { + ArrayList targetPoints = getTargetPoints(); + + 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 = high ? highPoint : midPoint; + double tapeHeight = high ? VisionConstants.HIGH_TAPE_HEIGHT : VisionConstants.MID_TAPE_HEIGHT; + double targetHeight = high ? VisionConstants.HIGH_TARGET_HEIGHT : VisionConstants.MID_TARGET_HEIGHT; + + double theta = VisionConstants.LIME_ANGLE + getPointAngle(tapePoint); + + double effectiveTapeHeight = tapeHeight - VisionConstants.LIME_HEIGHT; + double effectiveTargetHeight = targetHeight - VisionConstants.LIME_HEIGHT; + + double distanceToTape = effectiveTapeHeight / Math.sin(Math.toRadians(theta)); + + double distanceToTarget = effectiveTargetHeight * distanceToTape / effectiveTapeHeight; + + return distanceToTarget; } @Override From 8ee42e2421368cf27fad90f777e49553d45403c7 Mon Sep 17 00:00:00 2001 From: Abhishrek05 Date: Wed, 15 Mar 2023 14:55:18 -0600 Subject: [PATCH 3/7] abho is dead --- src/main/java/frc4388/robot/subsystems/Limelight.java | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index 9c91afb..a940777 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -67,7 +67,7 @@ public class Limelight extends SubsystemBase { return (VisionConstants.LIME_VIXELS - point.y) * (VisionConstants.V_FOV / VisionConstants.LIME_VIXELS); } - public double getDistanceToTarget(boolean high) throws AbhiIsADumbass { + public double getHorizontalDistanceToTarget(boolean high) throws AbhiIsADumbass { ArrayList targetPoints = getTargetPoints(); Point highPoint = targetPoints.get(0).y <= targetPoints.get(1).y ? targetPoints.get(0) : targetPoints.get(1); @@ -75,18 +75,16 @@ public class Limelight extends SubsystemBase { Point tapePoint = high ? highPoint : midPoint; double tapeHeight = high ? VisionConstants.HIGH_TAPE_HEIGHT : VisionConstants.MID_TAPE_HEIGHT; - double targetHeight = high ? VisionConstants.HIGH_TARGET_HEIGHT : VisionConstants.MID_TARGET_HEIGHT; double theta = VisionConstants.LIME_ANGLE + getPointAngle(tapePoint); double effectiveTapeHeight = tapeHeight - VisionConstants.LIME_HEIGHT; - double effectiveTargetHeight = targetHeight - VisionConstants.LIME_HEIGHT; double distanceToTape = effectiveTapeHeight / Math.sin(Math.toRadians(theta)); - double distanceToTarget = effectiveTargetHeight * distanceToTape / effectiveTapeHeight; + double horizontalDistanceToTarget = Math.sqrt(Math.pow(distanceToTape, 2) - Math.pow(effectiveTapeHeight, 2)); - return distanceToTarget; + return horizontalDistanceToTarget; } @Override From f0b14cebf42025287c12ed1e143f44e7576f7c7f Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 15 Mar 2023 15:25:06 -0600 Subject: [PATCH 4/7] lime align hopefully done --- src/main/java/frc4388/robot/Constants.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 7 ++-- .../frc4388/robot/commands/AutoBalance.java | 12 ------ .../frc4388/robot/commands/LimeAlign.java | 42 +++++++++++-------- .../commands/PelvicInflammatoryDisease.java | 8 ++-- .../frc4388/robot/subsystems/Limelight.java | 4 +- 6 files changed, 35 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index d58160e..1994a4f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -144,7 +144,7 @@ public final class Constants { } public static final class VisionConstants { - public static final String NAME = "Aarav is poopy"; + public static final String NAME = "photonCamera"; public static final int LIME_HIXELS = 640; public static final int LIME_VIXELS = 480; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1b6fb4c..ddf68d9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,10 +7,6 @@ package frc4388.robot; -import com.ctre.phoenix.motorcontrol.TalonFXControlMode; - -import edu.wpi.first.wpilibj.PWM; -import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -21,6 +17,7 @@ import frc4388.robot.commands.AutoBalance; import frc4388.robot.commands.JoystickPlayback; import frc4388.robot.subsystems.Arm; 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; @@ -50,6 +47,8 @@ public class RobotContainer { public final Claw m_robotClaw = new Claw(m_robotMap.servo); + public final Limelight m_limeLight = new Limelight(); + /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java index cace0a8..629c903 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/AutoBalance.java @@ -38,16 +38,4 @@ public class AutoBalance extends PelvicInflammatoryDisease { if (Math.abs(getError()) < 3) out2 = 0; drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false); } - - @Override - public void initialize() { - super.initialize(); - // this.gyro.reset(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } } diff --git a/src/main/java/frc4388/robot/commands/LimeAlign.java b/src/main/java/frc4388/robot/commands/LimeAlign.java index c811a3e..289a56c 100644 --- a/src/main/java/frc4388/robot/commands/LimeAlign.java +++ b/src/main/java/frc4388/robot/commands/LimeAlign.java @@ -4,31 +4,39 @@ package frc4388.robot.commands; -import edu.wpi.first.wpilibj2.command.CommandBase; +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 CommandBase { - public LimeAlign(SwerveDrive drive) { - addRequirements(drive); - } +public class LimeAlign extends PelvicInflammatoryDisease { - // Called when the command is initially scheduled. - @Override - public void initialize() { + SwerveDrive drive; + Limelight lime; + + + public LimeAlign(SwerveDrive drive, Limelight lime) { + super(0.1, 0.0, 0.0, 0.0, 10); + + this.drive = drive; + this.lime = lime; + addRequirements(drive, lime); } - // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public double getError() { + try { + return lime.getTargetPoints().get(0).x - (VisionConstants.LIME_HIXELS / 2); + } catch (AbhiIsADumbass abhiIsADumbass) { + abhiIsADumbass.printStackTrace(); + return 0; + } + } - // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; + public void runWithOutput(double output) { + drive.driveWithInput(new Translation2d(output, 0.0), new Translation2d(0.0, 0.0), true); } } diff --git a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java index 6c54024..a3c9e6c 100644 --- a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java +++ b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java @@ -4,7 +4,6 @@ package frc4388.robot.commands; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.utility.Gains; @@ -26,12 +25,13 @@ public abstract class PelvicInflammatoryDisease extends CommandBase { /** produces the error from the setpoint */ public abstract double getError(); + /** figure it out bitch */ public abstract void runWithOutput(double output); // Called when the command is initially scheduled. @Override - public void initialize() { + public final void initialize() { output = 0; } @@ -39,7 +39,7 @@ public abstract class PelvicInflammatoryDisease extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() { + public final void execute() { double error = getError(); cumError += error * .02; // 20 ms double delta = error - prevError; @@ -54,7 +54,7 @@ public abstract class PelvicInflammatoryDisease extends CommandBase { // Returns true when the command should end. @Override - public boolean isFinished() { + public final boolean isFinished() { return Math.abs(getError()) < tolerance; } } diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index a940777..e8b8756 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -14,14 +14,14 @@ import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; -import edu.wpi.first.math.Pair; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.VisionConstants; import frc4388.utility.AbhiIsADumbass; public class Limelight extends SubsystemBase { private PhotonCamera cam; - /** Creates a new LImelight. */ + + /** Creates a new Limelight. */ public Limelight() { cam = new PhotonCamera(VisionConstants.NAME); } From c85e70b88c95e086eebaffa479c2705efa51874f Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 15 Mar 2023 16:09:41 -0600 Subject: [PATCH 5/7] gud constants --- src/main/java/frc4388/robot/Constants.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 1994a4f..39b6c3f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -152,13 +152,13 @@ public final class Constants { public static final double H_FOV = 59.6; public static final double V_FOV = 45.7; - public static final double LIME_HEIGHT = -1.0; - public static final double LIME_ANGLE = -1.0; + public static final double LIME_HEIGHT = 6.0; + public static final double LIME_ANGLE = 55.0; - public static final double HIGH_TARGET_HEIGHT = -1.0; - public static final double HIGH_TAPE_HEIGHT = -1.0; + // public static final double HIGH_TARGET_HEIGHT = 46.0; + public static final double HIGH_TAPE_HEIGHT = 44.0; - public static final double MID_TARGET_HEIGHT = -1.0; - public static final double MID_TAPE_HEIGHT = -1.0; + // public static final double MID_TARGET_HEIGHT = 34.0; + public static final double MID_TAPE_HEIGHT = 32.0; } } From ff2c34a26e0b03827d554558c07c39bb74e39e6a Mon Sep 17 00:00:00 2001 From: Aarav Date: Wed, 15 Mar 2023 16:46:06 -0600 Subject: [PATCH 6/7] fix? --- .../java/frc4388/robot/RobotContainer.java | 7 ++--- .../frc4388/robot/subsystems/Limelight.java | 30 +++++++++++++++---- 2 files changed, 27 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ddf68d9..f7e61c7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -144,11 +144,8 @@ public class RobotContainer { - new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - .onTrue(new PivotCommand(m_robotArm, 135)); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) - .onTrue(new PivotCommand(m_robotArm, 210)); + // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + // .onTrue(new InstantCommand(() -> m_limeLight.toggleLEDs(), m_limeLight)); new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) .onTrue(new InstantCommand(() -> m_robotClaw.toggle())); diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index e8b8756..18250a2 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -14,6 +14,7 @@ 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; import frc4388.robot.Constants.VisionConstants; import frc4388.utility.AbhiIsADumbass; @@ -21,13 +22,24 @@ import frc4388.utility.AbhiIsADumbass; 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); } public void setLEDs(boolean on) { - cam.setLED(on ? VisionLEDMode.kOn : VisionLEDMode.kOff); + lightOn = on; + cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); + } + + public void toggleLEDs() { + lightOn = !lightOn; + cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); } public void setDriverMode(boolean driverMode) { @@ -35,6 +47,8 @@ public class Limelight extends SubsystemBase { } public ArrayList getTargetPoints() throws AbhiIsADumbass { + if (!cam.isConnected()) return null; + PhotonPipelineResult result = cam.getLatestResult(); if (!result.hasTargets()) throw new AbhiIsADumbass(); @@ -69,12 +83,13 @@ public class Limelight extends SubsystemBase { public double getHorizontalDistanceToTarget(boolean high) throws AbhiIsADumbass { 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 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 = high ? highPoint : midPoint; - double tapeHeight = high ? VisionConstants.HIGH_TAPE_HEIGHT : VisionConstants.MID_TAPE_HEIGHT; + Point tapePoint = targetPoints.get(0);//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); @@ -90,5 +105,10 @@ public class Limelight extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run + try { + System.out.println(getHorizontalDistanceToTarget(false)); + } catch(AbhiIsADumbass abhiIsADumbass) { + abhiIsADumbass.printStackTrace(); + } } } From 6b2ca9e4332fb74dc84ab3fadf6b97d9bb69e758 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 16 Mar 2023 16:25:26 -0600 Subject: [PATCH 7/7] 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++; + } }