Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 00:37:59 -06:00
Code Issues Packages Projects Releases Wiki Activity

abho is dead

Browse Source
This commit is contained in:
Abhishrek05
2023-03-15 14:55:18 -06:00
parent 395f78ed50
commit 7bda388d8a
1 changed files with 3 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Limelight.java
+3 -5
View File
@@ -67,7 +67,7 @@ public class Limelight extends SubsystemBase {
return (VisionConstants.LIME_VIXELS - point.y) * (VisionConstants.V_FOV / VisionConstants.LIME_VIXELS); 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<Point> targetPoints = getTargetPoints(); ArrayList<Point> targetPoints = getTargetPoints();
Point highPoint = 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);
@@ -75,18 +75,16 @@ public class Limelight extends SubsystemBase {
Point tapePoint = high ? highPoint : midPoint; Point tapePoint = high ? highPoint : midPoint;
double tapeHeight = high ? VisionConstants.HIGH_TAPE_HEIGHT : VisionConstants.MID_TAPE_HEIGHT; 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 theta = VisionConstants.LIME_ANGLE + getPointAngle(tapePoint);
double effectiveTapeHeight = tapeHeight - VisionConstants.LIME_HEIGHT; double effectiveTapeHeight = tapeHeight - VisionConstants.LIME_HEIGHT;
double effectiveTargetHeight = targetHeight - VisionConstants.LIME_HEIGHT;
double distanceToTape = effectiveTapeHeight / Math.sin(Math.toRadians(theta)); 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 @Override