Recommendations for Pathfinder + Odometry?

Hi all, I am trying to set my team up to follow paths using pathfinder and odometry. I found an excellent tutorial on YouTube that goes over a simple way to implement the WPILIB classes on a robot with 4 Neos as a drive train (what we use) but I wanted to ask here if anyone could share their experience/anything they learned throughout the process. I’ve noticed that throughout any of the large scale attempts to swap to a new method of programming is made by a team I am on, we normally get hung up on a problem that we can’t seem to figure out what is wrong and turn to Chief Delphi to solve our questions. I figured this time I would ask preemptively if anyone has any discoveries or words of advice that they could share before I waste days trying to get something working on our robot. Thanks in advance!

First: Set your robot up in simulation (but… REV’s simulation support is… eh, good luck) and do your debugging there. This will help you figure out logical issues with your code + trajectory stuff before having to deal with hardware issues.

Second: characterize your robot using sysid, and plug those numbers into your simulation. Once you get it right, your simulated trajectories will be pretty close to the real thing, and you’ll only need to do some minimal tweaking with real hardware.

If you’re using RobotPy, there’s a fully working ramsete command example (with simulation) that does an S pattern: examples/commands-v2/ramsete at main · robotpy/examples · GitHub . If you’re not using RobotPy, just translate it into whatever language you’re using.


We’ve had very good success with it. I recommend you use a trajectory generator like PathPlanner to help you rapidly draw, visualize, and deploy trajectories (You can tune your autos for the differences in different carpets in ~2 runs, sufficient for practice matches. Take your time when characterizing; good constants make the difference between a good trajectory following robot and a bad one. Make sure to set up your SysID tool correctly, use units in meters, and make sure to set your encoder delay in the SysID analyzer. Make a helper method to generate trajectory following commands so you can easily write one into a command group.
Good luck!

Thank you for the response! Just to confirm, you are using the SysID tool provided by WPILIB at this link correct? I just want to get started on the right foot, I was not sure if there are other SysID tools teams use or if the preferred method is just to use the one from WPILIB. Also, would you please elaborate a little bit more on what you mean by creating a helper method to generate your trajectories? I am familiar with command-style programming in FRC and I use command groups heavily, I just would like a little more clarification for my own sake. Thank you!

I’m using the instructions from that link; the Sysid tool is installed on your computer when you install WPILib. Some teams write their own, but that is beyond my team’s level of expertise, and the custom ones don’t work better, they just generate fewer constants and need less data. (6328 discussed theirs in their build thread.)

What I mean by a helper method is a method that generates the trajectory command from a trajectory argument. It is a function with a Command return type and parameters of a Trajectory or whatever data you need to generate the trajectory, if you want to roll the whole thing into one. My team wrote a full helper class with two methods that we use: Command createTrajectoryCommand(Trajectory), generateFromPathPlanner(trajectoryJSONPath).


public final static Trajectory generateFromPathPlanner(String trajectoryJSON) { trajectoryJSON = "pathplanner/generatedJSON/"+trajectoryJSON+".wpilib.json"; Trajectory trajectory; try { Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON); trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath); } catch (IOException ex) { DriverStation.reportError("Unable to open trajectory: " + trajectoryJSON, ex.getStackTrace()); TrajectoryConfig config = new TrajectoryConfig(1, 1); trajectory= TrajectoryGenerator.generateTrajectory(new Pose2d(), List.of(), new Pose2d(), config); } return trajectory; }


public final static Command createTrajectoryCommand(Trajectory _trajectoryToFollow) { var leftController = new PIDController(Constants.kP, 0, 0); var rightController = new PIDController(Constants.kP, 0, 0); //RobotContainer.s_drivetrain.getField2d().getObject("traj").setTrajectory(_trajectoryToFollow); RamseteController ramseteController = new RamseteController(Constants.kRamseteB, Constants.kRamseteZ); RamseteCommand ramseteCommand = new RamseteCommand( _trajectoryToFollow, RobotContainer.s_drivetrain::getPose, ramseteController, new SimpleMotorFeedforward(Constants.ks, Constants.kv,Constants.ka), Constants.kDriveKinematics, RobotContainer.s_drivetrain::getWheelSpeeds, leftController, rightController, (leftVolts, rightVolts) -> { RobotContainer.s_drivetrain.tankDriveVolts(leftVolts, rightVolts); }, RobotContainer.s_drivetrain); return new AddTrajectoryToField(RobotContainer.s_drivetrain.getField2d(), _trajectoryToFollow).andThen(ramseteCommand); }

Please note these two methods won’t work if you just put them in your code as they are quite dependent on methods within our drivetrain subsytem. We followed WPILib’s Trajectory Tutorial to build our drivetrain subsystem and then put all the trajectory following code into the helper method. Essentially the helper method just makes it easier to make a trajectory command than it would be if you had to write the whole thing every time.

Good luck!

1 Like

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