Swerve Pose Estimator

Technical
1

Does anyone know how I could create a Pose Estimator for swerve? I have created a PID loop for turning, and would like to create one for moving, but I need to create a pose estimator. My code is at GitHub - Mateo-Johnson/RobustSturdy2. All the best!

2

I would just use the class WPILib already has instead of making an entirely custom one SwerveDrivePoseEstimator (WPILib API 2024.2.1)

3

So few questions and excuse my ignorance but we are in early stages of trying this.
Few things I am trying to wrap head around.

  1. Does this estimator maintain 0,0 in the same spot on field regardless of alliance color?
  2. It looks to me like you want to update it every run through program. Is it best to add a vision measurement every time through as well as long as a valid target is found?
  3. If so and using a limelight … (this probably ties to first question). Should you be using just get bot pose or one of the wpi blue and red methods based on which alliance you are on?
4

  1. I’m not exactly sure what you’re asking but once you update the pose estimator once it’s probably not going to be equal to your starting pose anymore

  2. Yeah you want to update it as much as possible to keep your odometry and estimated vision pose in sync as much as possible

  3. Probably just get bot pose unless you have a specific reason to be using wpiblue or wpired

5

1 - yes
2 - yes
3 - always use blue (ok I’m pretty sure blue… but definitely always use the same one!)

I would suggest experimenting with the field2d object in shuffleboard, etc. it important to understand where the robot is and where the robot thinks it is.

1 Like
6

Yeah, that’s what I was thinking, could you give me some example code? I can’t get it figured out?

7

I’m not very familiar with Limelight Lib but if you want my this year’s robot code (which uses 4 cameras on PhotonLib) i can give that. We did have a vision subsystem in 2023 that used Limelight but it never got used but it was actually updating our pose.

2024 Robot Code (using PhotonVision): GitHub - frc1806/Crescendo-1806: FRC Team 1806's 2024 Robot Code

2023 Robot Code (using Limelight): GitHub - frc1806/ChargedUp-1806: FRC 1806's code for the 2023 competition season

8

Wait, so I’ll only be able to update my pose when the robot sees an apriltag?

9

With SwervePoseEstimator yeah but you can also use SwerveDriveOdometry for when you don’t see a tag

10

Your pose estimator is a tool to blend your odometery and your vision poses as I understand it correct

11

Yeah. Also i apologise, I forgot my team used a tank drive in 2023 so there literally is no code example I have using Limelight and SwervePoseEstimator but there’s definitely tons of teams out there who did use both

12

In case you are still wondering. Limelight has a good example: limelight-examples/java-wpilib/swerve-megatag-odometry/src/main/java/frc/robot/Drivetrain.java at main · LimelightVision/limelight-examples · GitHub

13

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