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 the coding better

Browse Source
This commit is contained in:
Astatin3
2024-01-24 16:40:00 -07:00
parent f64c010936
commit 19b5d75272
2 changed files with 49 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Limelight.java
+21 -21
View File
@@ -130,35 +130,35 @@ public class Limelight extends SubsystemBase {
return distanceToApril;
}
public PhotonTrackedTarget getLowestTape() {
if (!cam.isConnected()) return null;
// public PhotonTrackedTarget getLowestTape() {
// if (!cam.isConnected()) return null;
PhotonPipelineResult result = cam.getLatestResult();
// PhotonPipelineResult result = cam.getLatestResult();
if (!result.hasTargets()) return null;
// if (!result.hasTargets()) return null;
ArrayList<PhotonTrackedTarget> points = (ArrayList<PhotonTrackedTarget>) result.getTargets();
// ArrayList<PhotonTrackedTarget> points = (ArrayList<PhotonTrackedTarget>) result.getTargets();
PhotonTrackedTarget lowest = points.get(0);
for (PhotonTrackedTarget point : points) {
if (point.getPitch() < lowest.getPitch()) {
lowest = point;
}
}
// PhotonTrackedTarget lowest = points.get(0);
// for (PhotonTrackedTarget point : points) {
// if (point.getPitch() < lowest.getPitch()) {
// lowest = point;
// }
// }
return lowest;
}
// return lowest;
// }
public double getDistanceToTape() {
PhotonTrackedTarget tapePoint = getLowestTape();
if (tapePoint == null) return -1;
// 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 tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT;
// double theta = 35.0 + tapePoint.getPitch();
double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta));
return distanceToTape;
}
// double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta));
// return distanceToTape;
// }
@Override
public void periodic() {}
src/main/java/frc4388/robot/subsystems/Vision.java
+28 -19
View File
@@ -7,32 +7,41 @@ import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
public class Vision {
private final NetworkTableEntry m_isTags;
private final NetworkTableEntry m_xPoses;
private final NetworkTableEntry m_yPoses;
private final NetworkTableEntry m_zPoses;
private final NetworkTableEntry nt_ll_tx;
private final NetworkTableEntry nt_ll_ty;
private final NetworkTableEntry nt_ll_ta;
private final NetworkTableEntry nt_ll_ts;
public Vision() {
final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
final var tagTable = NetworkTableInstance.getDefault().getTable("limelight`");
m_isTags = tagTable.getEntry("IsTag");
m_xPoses = tagTable.getEntry("TagPosX");
m_yPoses = tagTable.getEntry("TagPosY");
m_zPoses = tagTable.getEntry("TagPosZ");
nt_ll_tx = tagTable.getEntry("tx"); //TODO
nt_ll_ty = tagTable.getEntry("ty"); //TODO
nt_ll_ta = tagTable.getEntry("ta"); //TODO
nt_ll_ts = tagTable.getEntry("ts"); //TODO
// I do not know what these values represent
// There are diffrent values that are sent
// when "3D" mode is enabled on the limelight
// So this needs to be updated!
}
public AprilTag[] getAprilTags() {
if (!m_isTags.getBoolean(false)) return new AprilTag[0];
// public AprilTag[] getAprilTags() { This func should return an apriltag object!
public void getAprilTags() {
// if (!m_isTags.getBoolean(false)) return new AprilTag[0];
double xarr[] = m_xPoses.getDoubleArray(new double[] {});
double yarr[] = m_yPoses.getDoubleArray(new double[] {});
double zarr[] = m_zPoses.getDoubleArray(new double[] {});
double ll_tx = nt_ll_tx.getDouble(new Double(0));
double ll_ty = nt_ll_ty.getDouble(new Double(0));
double ll_ta = nt_ll_ta.getDouble(new Double(0));
double ll_ts = nt_ll_ts.getDouble(new Double(0));
AprilTag tags[] = new AprilTag[xarr.length];
for (int i = 0; i < tags.length; i++) {
tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d()));
}
// AprilTag tags[] = new AprilTag[xarr.length];
// for (int i = 0; i < tags.length; i++) {
// tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d()));
// }
return tags;
// return tags;
}
}