Team4388/2024AcrossTheRidgebotiverse
1
0
Fork 0
mirror of https://github.com/Team4388/2024AcrossTheRidgebotiverse.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

Make boiler plate code - Needs to be programmed with robot

Browse Source
This commit is contained in:
Astatin3
2024-02-15 10:27:44 -07:00
parent 19fb13e5d9
commit 1d04fc34de
11 changed files with 125 additions and 241 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Limelight.java
+18 -148
View File
@@ -3,163 +3,33 @@
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.subsystems;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import org.photonvision.EstimatedRobotPose;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
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.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.VisionConstants;
// Look at vvv for networktables stuff
// https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#apriltag-and-3d-data
public class Limelight extends SubsystemBase {
private PhotonCamera cam;
private PhotonPoseEstimator photonPoseEstimator;
private double[] cameraPose;
private boolean lightOn;
public Pose2d getPose() {
//TODO - Get actual values!
/** Creates a new Limelight. */
public Limelight() {
cam = new PhotonCamera(VisionConstants.NAME);
cam.setDriverMode(false);
}
double x = 0;
double y = 0;
double yaw = 0;
public void setLEDs(boolean on) {
lightOn = on;
cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
}
Rotation2d rot = Rotation2d.fromDegrees(yaw);
public void toggleLEDs() {
lightOn = !lightOn;
cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
}
public void setDriverMode(boolean driverMode) {
cam.setDriverMode(driverMode);
}
public void setToLimePipeline() {
cam.setPipelineIndex(1);
setLEDs(true);
}
public void setToAprilPipeline() {
cam.setPipelineIndex(0);
setLEDs(false);
}
public PhotonTrackedTarget getAprilPoint() {
if (!cam.isConnected()) return null;
PhotonPipelineResult result = cam.getLatestResult();
if (!result.hasTargets()) return null;
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;
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() {
if (!cam.isConnected()) return null;
PhotonPipelineResult result = cam.getLatestResult();
if (!result.hasTargets()) return null;
ArrayList<PhotonTrackedTarget> points = (ArrayList<PhotonTrackedTarget>) result.getTargets();
PhotonTrackedTarget lowest = points.get(0);
for (PhotonTrackedTarget point : points) {
if (point.getPitch() < lowest.getPitch()) {
lowest = point;
}
}
return lowest;
}
public double getDistanceToTape() {
PhotonTrackedTarget tapePoint = getLowestTape();
if (tapePoint == null) return -1;
double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT;
double theta = 35.0 + tapePoint.getPitch();
double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta));
return distanceToTape;
return new Pose2d(x, y, rot);
}
@Override
public void periodic() {}
}
public void periodic() {
// This method will be called once per scheduler run
cameraPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("camerapose_targetspace").getDoubleArray(new double[6]);
}
}