The optimization of Limelight

Our problem about limelight is that when l press the limelight button, the robot will turn but it won’t drive to the desired distance , so how to adjust my limelight code . And my code is below

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.subsystems.limelight;

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.Robot;
import frc.robot.util.Constants;
import frc.robot.util.RobotContainer;

public class limelighton extends CommandBase {
  static double test = 0;
  /** Creates a new limelight. */
  public limelighton() {
    // Use addRequirements() here to declare subsystem dependencies.
    addRequirements(RobotContainer.m_Drive);
  }

  // Called when the command is initially scheduled.
  @Override
  public void initialize() {
  }

  // Called every time the scheduler runs while the command is scheduled.
  @Override
  public void execute() {

    // Get tx 
    double lime_tx = RobotContainer.m_Drive.Get_tx();

    //  Set the parameters
    double heading_error = - RobotContainer.m_Drive.Get_tx();
    // You need to reset the cross_hair in order to use this method . Unless you will need to calculate the current distance.
    double distance_error = - RobotContainer.m_Drive.Get_ty(); 

    // Package the parameters
    double KpAim = Constants.Limelight.KpAim;
    double KpDistance = Constants.Limelight.KpDistance;

    // Package the degree of drive
    double min_command = Constants.Limelight.min_command;

    // Set some crucial parameters
    double steering_adjust = 0.0;
    double left_command = 0.0;
    double rght_command = 0.0;
    double distance_adjust = 0.0;

    if(lime_tx>1.0){
      steering_adjust = KpAim * heading_error - min_command;
    }
    else if(lime_tx < 1.0){
      steering_adjust = KpAim * heading_error + min_command;
    }

    distance_adjust = KpDistance * distance_error;

    left_command += steering_adjust + distance_adjust;
    rght_command += steering_adjust + distance_adjust;

    Robot.hardware.m_diffDrive.tankDrive(left_command, rght_command);
    test ++;
    SmartDashboard.putNumber("test",test);

  }

  // Called once the command ends or is interrupted.
  @Override
  public void end(boolean interrupted) {
  }

  // Returns true when the command should end.
  @Override
  public boolean isFinished() {
    return false;
  }
}

A couple questions to help “flesh out” what’s actually going on here:

What is this value, numerically?

How did you pick that value?

Can you describe for us what the value contained in this variable should represent? Use physical measurements (not other variable names) if possible.

Over time, how do you expect its value to behave? Should its value go up, go down, go toward zero, not change much, or something else?

And, finally, find a way to see what that value is actually doing (plot in Glass/Shuffleboard, print it out, debugger, anything). Does its behavior match your expectations? Why or why not?


        // Set PID parameters.
        public static final double KpAim = -0.1;
        public static final double KpDistance = -0.1;

        public static final double min_command = 0.05;

l copy the code from the official site , but l doesn’t work very well since it can’t reach my desired distance from my robot to the target.

Hmmm ok. I assume it’s this documentataion page? A few thoughts:

The optimal values for PID constants will depend on the underlying electronics hardware of the robot. Since Limelight doesn’t know about your robot, I wouldn’t necessarily trust their values to be the correct ones. While the sample values from Limelight’s docs might be decent starting points, they won’t necessarily be correct for your robot.

The distance_error part of my question is designed to help step through some of the necessary debug steps to move from the symptom you observed to one or more root causes. It will also be needed to help understand whether the KpDistance number needs to be adjusted, and how.

See this page for more information on PID controller tuning.

So, how to adjust the distance_error code ?

1 Like

I don’t know :slight_smile: . That’s what the questions are designed to help you uncover.

From your other thread:

It’s mostly a thought exercise:

  1. What should distance_error represent, physically?
  2. How should it’s value change over time?

Then, run the experiment. Does it match your expectations? If not, trace backward to find another issue.

Good Engineering is the real-time application of the Scientific Method.

Maybe it’s a parameter to help aim at the target . And can you send a perfect limelight code to me ?

Nope :smiley: .

  1. I don’t know what you want your code to do, nor do I know anything about your robot.
  2. It is better to know how to solve problems, rather than simply having one answer.

Perhaps. What leads you to this conclusion?

Some additional, related, and specific questions to chew on: What variables and math operations are used while assigning a value to distance_error every loop? How do they get their values? What do their values represent in the physical world?

3 Likes

Is there any easier way to adjust the limelight’s pid parameters

Hmmmm.

In general, for a PID controller with the I and D gains set to zero, the qualitative behavior you should expect:

  1. Robot does not move with Kp set to zero.
  2. Small, nonzero Kp values will cause the robot to move very sluggishly, possibly not getting to the target.
  3. Large Kp values will cause the robot to oscillate, overshoot the target, or generally “go crazy”.

Tuning is nothing more than increasing or decreasing the magnitude of Kp until the behavior falls somewhere between (2) and (3), and is “nice”.

My main caveat remains: All this assumes the code is written correctly before attempting tuning. The questions I was posing above were attempting to work you toward that stage, before tuning. I am worried you are going to have issues if you don’t investigate what your code is supposed to be doing before attempting to do PID tuning.

Here’s the main difficulty I’ve for FRC teams: The SYMPTOM of the robot not moving or “acting crazy” has at least two possible ROOT CAUSES:

  1. PID gains not correct.
  2. Software bug elsewhere.

By symptom alone, it is not possible to identify which root cause is the source of the issues.

PID is deceptively difficult. A good PID controller can be done in under 10 lines of code. The tuning process can be done manually, with no knowledge of the underlying math. That’s why it’s “in reach” for many FRC teams. However. The reason those 10 lines of code are the correct ones, and the reasons it works well are a college undergraduate or graduate topic.

The issues I commonly see students have often start with the assumption that because the number of lines of code is small, or because it worked well on someone else’s robot for their desired outcomes, it will work well on their own robot and their own desired outcomes. Be careful not to fall into this mindset.

Some additional resources to consider:

  1. The bottom of this page has a section named “PID Tuning Method” which lists out a few of the common tuning techniques.
  2. There’s a free textbook written on these topics, in the context of FRC.
  3. The FRC Characterization Utility allows you to extract some physical parameters out of your hardware which could be useful. I’ve not heard of someone doing it specifically for vision processing yet.
  4. I did a write-up and simulation a while back that’s mildly relevant, which might shed some light on the behaviors you should be expecting as you turn your Kp “knobs”
  5. Youtube has many videos on people doing similar things, if you like that learning flow. In particular, the FRC Zero to Autonomous series covers a number of relevant concepts.
2 Likes

l think the KpAim and KpDistance are just two proportional parameters , and can you use the same method to tune it just like tuning the PID parameters . And my another question is how to tune the PID parameters precisely , after seeing the WPI doc, l just don’t know how to tune it . For example, how to tune the PID parameters of Limelight . And what’s the setpoint of it ?

I concur, they represent the proportional gain in PID controllers with I and D hardcoded to zero.

Sometimes it’s a variable that’s an input to the PID controller. Other times it is hardcoded.

Keep in mind inside of most PID controllers, error is calculated as setpoint - actual.

Referencing this example from Limelight, I can show you one way to identify it. Note that the example calculates the error as such:

        float heading_error = -tx;
        float distance_error = -ty;

Exercise for the user:

  1. What numeric value of setpoint would make the example’s code line up with the above definition for PID error?
  2. What would a setpoint of that value physically mean? IE, what can you say about the positions of the limelight, the robot, and the target when tx and ty are equal to the setpoints?

The best summary I can give to get started is the one I posted above:

Have you attempted adjusting the Kp value yet? Do all three observations line up? Have you found an intermediate value which performs nicely on your robot hardware?

Thanks , but we use PhotonVision instead of Limelight. And the code is below :

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.subsystems.limelight;

import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.util.Units;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.Robot;
import frc.robot.util.RobotContainer;

import org.photonvision.PhotonCamera;
import org.photonvision.PhotonUtils;

public class photonlime extends CommandBase {
  /** Creates a new photonlime. */
  // Constants such as camera and target height stored. Change per robot and goal!
  final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(48.1 / 2.54);
  final double TARGET_HEIGHT_METERS = Units.feetToMeters(8) + Units.inchesToMeters(2.25);
  // Angle between horizontal and the camera.
  final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(60);
      
  // How far from the target we want to be
  final double GOAL_RANGE_METERS = Units.feetToMeters(3);

  PhotonCamera m_camera = new PhotonCamera("photonvision");

  // PID constants should be tuned per robot
  final double LINEAR_P = 0.1;
  final double LINEAR_D = 0.0;
  PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D);
  
  final double ANGULAR_P = 0.1;
  final double ANGULAR_D = 0.0;
  PIDController turnController = new PIDController(ANGULAR_P, 0, ANGULAR_D);
  
  public photonlime() {
    // Use addRequirements() here to declare subsystem dependencies.
    addRequirements(RobotContainer.m_Drive);
  }

  // Called when the command is initially scheduled.
  @Override
  public void initialize() {

  }

  // Called every time the scheduler runs while the command is scheduled.
  @Override
  public void execute() {
    double forwardSpeed;
    double rotationSpeed;

    // Vision-alignment mode
    // Query the latest result from PhotonVision
    var result = m_camera.getLatestResult();

    if (result.hasTargets()) {
        // First calculate range
        double range =
                PhotonUtils.calculateDistanceToTargetMeters(
                        CAMERA_HEIGHT_METERS,
                        TARGET_HEIGHT_METERS,
                        CAMERA_PITCH_RADIANS,
                        Units.degreesToRadians(result.getBestTarget().getPitch()));

        // Use this range as the measurement we give to the PID controller.
        // -1.0 required to ensure positive PID controller effort _increases_ range
        forwardSpeed = -1.0 * forwardController.calculate(range, GOAL_RANGE_METERS);

        // Also calculate angular power
        // -1.0 required to ensure positive PID controller effort _increases_ yaw
        rotationSpeed = -1.0 * turnController.calculate(result.getBestTarget().getYaw(), 0);
      } else {
          // If we have no targets, stay still.
          forwardSpeed = 0;
          rotationSpeed = 0;
        } 
      // Use our forward/turn speeds to control the drivetrain
      Robot.hardware.m_diffDrive.arcadeDrive(forwardSpeed,rotationSpeed);
    }




  // Called once the command ends or is interrupted.
  @Override
  public void end(boolean interrupted) {}

  // Returns true when the command should end.
  @Override
  public boolean isFinished() {
    return false;
  }
}

And how to tune its PID parameters.

I would not expect that changing from Limelight’s software to Photonvision’s software will not change the fundamental tuning process.

Can you elaborate?

  1. What steps have you taken?
  2. What are the behaviors you are currently seeing?
    • What do you believe is correct, and why?
    • What do you believe is incorrect, and why?