We’re using a LimeLight + AprilTag detection for our autonomous routine, basically the limelight detects an apriltag, does some math to determine the distance from it, and moves backward accordingly.
We based our code off of Estimating Distance | Limelight Documentation.
Our distance estimation code is done inside a function with 3 params:
public static double estimateDistance(double mountAngleDegrees, double lensHeightInches, double goalheightInches) {
double verticalOffset = getVerticalOffset();
double angleToGoalDegrees = mountAngleDegrees + verticalOffset;
double angleToGoalRadians = angleToGoalDegrees * (3.14159/ 180);
double distance = (goalheightInches - lensHeightInches) / Math.tan(angleToGoalRadians);
return distance;
}
We have a couple other helper functions too that assist with our command, they all just fetch specific data from NetworkTables and returns it.
Our getting in range code pretty much looks like this:
while(LimeLightCamera.hasValidTargets() >= 1) {
System.out.println(LimeLightCamera.estimateDistance(LimelightConstants.MountAngleDegrees,
LimelightConstants.LensHeightInches, LimelightConstants.GoalHeightInches));
if(LimeLightCamera.estimateDistance(LimelightConstants.MountAngleDegrees,
LimelightConstants.LensHeightInches, LimelightConstants.GoalHeightInches) < 40) {
if(LimeLightCamera.hasValidTargets() >= 1) {
driveTrain.drive(.30); //.25
}
}
}
However, the robot will continue to drive, even after reaching well past where it should stop. It just keeps moving forever, and I’m clueless as to whats causing this;
-
we’ve double-checked that we have the correct angle, lens height inches, and height inches of the AprilTag.
-
we printed out the distance to console periodically when we run the command, it does go over 40 inches, but the robot keeps moving. This check should have kept it from doing that, but for some reason it fails?
...
if(LimeLightCamera.estimateDistance(LimelightConstants.MountAngleDegrees,
LimelightConstants.LensHeightInches, LimelightConstants.GoalHeightInches) < 40)
...
- We double checked our limelight configurations, and they’re definitely valid. So I have no clue whats wrong