Help with auto; robot still moves after reaching the correct distance

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;

  1. we’ve double-checked that we have the correct angle, lens height inches, and height inches of the AprilTag.

  2. 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)
...
  1. We double checked our limelight configurations, and they’re definitely valid. So I have no clue whats wrong

Your while() loop instructs the robots to drive but when you exit it you never tell the motors to stop. That’s the first thing that jumps out to me.

I’m also not sure what framework you’re in that allows a while() loop like that to control the robot. Seems like that would cause a scheduler overrrun and disable the bot right quick.

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.