How use limelight to Aiming and Ranging With Swerve

Hii, I am new in this foros, I´m sorry for my English but I have some questions about LimeLight, because, My team is new with this of Vision, and new use Swerve, we are using the librery “YAGSL” to Swerve, and now we want that the robot Aiming with the AprilTags, I have been see diferentes codes, but I can´t understand how applicate the limelight with we code.

Pliss a need of your help, because I new with this of and I want contribute a team with this. Here I leave the code that I am using. If you see some error, plis said me what is that, and how I can fix.

An easy way to do this is to have a PID controller that controls the rotational rate of the robot based on the measurement of your Limelight’s tX value.

PIDController controller = new PIDController(5, 0, 0);
controller.enableContinuousOutput(-Math.PI, Math.PI);

public Command autoAlign(DoubleSupplier xSpeed, DoubleSupplier ySpeed) {
    return this.driveCommandu(xSpeed, ySpeed, controller.calculate(Units.degreesToRadians(LimelightHelpers.getTX("limelight")), 0.0));
    // I'm fairly sure that degreesToRadians is necessary since you are enabling continous output using radians, but if someone could clarify that, that would be nice
}

If you want it to only align to the center speaker AprilTag then you can configure the Limelight to filter out the side tag ids. Let me know if you need any other help with this.

1 Like

OKey, but this code the in the “Swervesubsystem” no?

1 Like

Yes, wherever your drive command is located

Thank you, Now the problem is a error in the code.

This I don´t know how fix this, I think that the last date is a value double, and the command “driveCommandu” use DoubleSuplier and I think that I need change this value a DoubleSupplier. I think, but I don´t sure

Yes sorry, I missed that, that needs to be a double supplier. My bad, I should’ve referenced our code.

1 Like

Other question, is the same if I want Raging with swerve, but Now I use the values of xspeed and yspeed?, and I need created new PID for these section, Right?

I don’t quite understand what you mean by raging (language barriers are fun) but yes, if you want to control the x and y values just like you did with rotation you would just create more pid loops and use those for the x and y suppliers

I´m sorry jajajjajaja, this is very funny jajajajja,

I try do this, is like range, like if your robot need have a range of distance to AprilTag, This I try.

  public Command autoRange(){
    return this.driveCommandu(()->0,()-> controller_range.calculate(Units.degreesToRadians(LimelightHelpers.getTX("limelight")), 0.0), ()->0);
  }

but Now I use the TY to calculate the range, but I don´t sure with this.

Ohhh gotcha sorry. A few different ways that you could do this could be using localization and path finding if you have that set up (that’s a whole other beast though that we ran out of time for this year). Otherwise another option could be to use trig. Limelight has a lot of good tutorials on how to do this, getting in range. Basically however you just use trig to get your distance to the tag then just have a pid loop to drive your distance error to whatever your set point is.

1 Like

Ohh, thanks, I try do this, anything or question that I have, I send message, thank you so much :smiley:

1 Like

Of course! Feel free to reach out if you have any more questions and I can try to help

HII, I am again here, with a question jsjsjsjs, first of foremost thank you to the solution that I had. Now I want that the robot to stand a certain distance away from AprilTag.

I saw this video and I want to achieve this
Video

I can do that the robot rotate to AprilTag (rot_limelight), the only thing missing is the distance (forward_limelight) . This is my command that I have.

private final PIDController controller = new PIDController(2.1, 0,0.01 ); // P:3
  private final PIDController forwardPidController = new PIDController(0.1, 0, 0);

  public double autoforward(){
    var  targetingFowardSpeed = forwardPidController.calculate(Units.degreesToRadians(LimelightHelpers.getTY("limelight")), 0.5);
    targetingFowardSpeed *=maximumSpeed;
    targetingFowardSpeed *= -1;
    return targetingFowardSpeed;
  }

  public Command drive_limelight(){
    final var rot_limelight = controller.calculate(Units.degreesToRadians(LimelightHelpers.getTX("limelight")),0);
    var rot = rot_limelight;

    final var forward_limelight = autoforward();
    var xspeed = forward_limelight;

    return this.driveCommandu(()->xspeed, ()-> 0,()->rot);
  }

I had seen the link of the comment of the video youtube, but I still do not understand how implement with my code. also I think that exist a better form to program these, because the code of the team is very complex. You know how help me?

Sorry but I don’t quite understand what you are asking. The method autoforward() is returning a double value that you are then passing to forward_limelight that then gets passed to your drive command. This is what is getting your x speed for the drive command to drive you to the certain distance away.

Sorry, to be more specific, I try that the robot always stays at a distance away from the AprilTag, but always pointing towards it, and I don’t know how I can do that in code.

like in this video:
Video

1 Like

It´s easier for you to do a pathfinding algorithm using pathplanner to accomplish it than programming your robot to align this way to the April tag. However, if you really want to do something like the video I would recommend you to check the GitHub link that is available in the description of the video, this way you will get how it works.