Skip to content

Pathfinder for FRC Java

Jump to bottom
Jaci Brunning edited this page Feb 21, 2019 · 5 revisions

This is the wiki entry for how to install and use Pathfinder for an FRC Java Project. This guide covers both Toast and WPILib installation, however the actual code and examples are the same for both.

Downloading Pathfinder

See the README for more information: https://github.com/JacisNonsense/Pathfinder#part-of-the-first-robotics-competition

Including Pathfinder in your Project

WPILib

Copy the Pathfinder-Java jar file to <user home>/wpilib/user/lib. In your project, open the build.properties file and uncomment the line #userLibs=${user.home}/wpilib/user/lib (remove the # from the front to uncomment).

NOTE: on Windows, <user.home> will be C:\Users\<your name>. On Linux, it will be /home/<your user>. On Mac, it will be /Users/<your user>.

Toast

Create a folder in your project called libs/ and copy the Pathfinder Jar there. GradleRIO will automatically pick up the dependency and load it.

Using the Library

Whenever you want to generate a trajectory, you can do so using the Pathfinder class.

Waypoint[] points = new Waypoint[] {
    new Waypoint(-4, -1, Pathfinder.d2r(-45)),      // Waypoint @ x=-4, y=-1, exit angle=-45 degrees
    new Waypoint(-2, -2, 0),                        // Waypoint @ x=-2, y=-2, exit angle=0 radians
    new Waypoint(0, 0, 0)                           // Waypoint @ x=0, y=0,   exit angle=0 radians
};

Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, Trajectory.Config.SAMPLES_HIGH, 0.05, 1.7, 2.0, 60.0);
Trajectory trajectory = Pathfinder.generate(points, config);

You can also modify the trajectory for either Swerve or Tank drive

TankModifier modifier = new TankModifier(trajectory).modify(0.5);
// OR
SwerveModifier modifier = new SwerveModifier(trajectory).modify(0.5, 0.5, SwerveModifier.Mode.SWERVE_DEFAULT);

More about this can be seen in the Pathfinder-Java README.

Following a Trajectory

To get your robot to follow a trajectory, you can use the EncoderFollower object. As the name suggests, this will use encoders as feedback to guide your robot along the trajectory. It is important that your time step passed into your Trajectory Configuration is the same as the time difference between control loop iterations, otherwise you may find your path tracking inaccurately.

Tank Drive

Create 2 EncoderFollower objects, one for the left and one for the right.

EncoderFollower left = new EncoderFollower(modifier.getLeftTrajectory());
EncoderFollower right = new EncoderFollower(modifier.getRightTrajectory());

When you're ready to start following: Setup your encoder details:

// Encoder Position is the current, cumulative position of your encoder. If you're using an SRX, this will be the
// 'getEncPosition' function.
// 1000 is the amount of encoder ticks per full revolution
// Wheel Diameter is the diameter of your wheels (or pulley for a track system) in meters
left.configureEncoder(encoder_position, 1000, wheel_diameter);

Set your PID/VA variables:

// The first argument is the proportional gain. Usually this will be quite high
// The second argument is the integral gain. This is unused for motion profiling
// The third argument is the derivative gain. Tweak this if you are unhappy with the tracking of the trajectory
// The fourth argument is the velocity ratio. This is 1 over the maximum velocity you provided in the 
//      trajectory configuration (it translates m/s to a -1 to 1 scale that your motors can read)
// The fifth argument is your acceleration gain. Tweak this if you want to get to a higher or lower speed quicker
left.configurePIDVA(1.0, 0.0, 0.0, 1 / max_velocity, 0);

Inside your control loop, you can add the following code to calculate the desired output of your motors:

double output = left.calculate(encoder_position);

Now, keep in mind this doesn't account for heading of your robot, meaning it won't track a curved path. To adjust for this, you can use your Gyroscope and the desired heading of the robot to create a simple, proportional gain that will turn your tracks. A full example, including the calculations for each side of the drive train is given below.

double l = left.calculate(encoder_position_left);
double r = right.calculate(encoder_position_right);

double gyro_heading = ... your gyro code here ...    // Assuming the gyro is giving a value in degrees
double desired_heading = Pathfinder.r2d(left.getHeading());  // Should also be in degrees

// This allows the angle difference to respect 'wrapping', where 360 and 0 are the same value
double angleDifference = Pathfinder.boundHalfDegrees(desired_heading - gyro_heading);
angleDifference = angleDifference % 360.0;
if (Math.abs(angleDifference) > 180.0) {
  angleDiff = (angleDifference > 0) ? angleDifference - 360 : angleDiff + 360;
} 

double turn = 0.8 * (-1.0/80.0) * angleDifference;

setLeftMotors(l + turn);
setRightMotors(r - turn);

The Pathfinder.boundHalfDegrees() function simply binds a degrees angle to -180..180, making sure we don't end up with an absurdly large turn value

Note that for the desired heading of the robot, we're only using the left follower as a comparison. This is because both the left and right sides of a tank drive are parallel, and therefore always face in the same direction.

Swerve Drive

Swerve Drive following is very similar to Tank Drive, except each wheel can have a different trajectory and heading. To make things simple, I will be showing how to do it for a single wheel. For all 4 wheels, just do the exact same thing 4 times.

Create an EncoderFollower object for your wheel:

EncoderFollower flFollower = new EncoderFollower(modifier.getFrontLeftTrajectory());   // Front Left wheel

When you're ready to start following: Setup your encoder details:

// Encoder Position is the current, cumulative position of your encoder. If you're using an SRX, this will be the
// 'getEncPosition' function.
// 1000 is the amount of encoder ticks per full revolution
// Wheel Diameter is the diameter of your wheel in meters
flFollower.configureEncoder(fl_encoder_position, 1000, wheel_diameter);

Set your PID/VA variables:

// The first argument is the proportional gain. Usually this will be quite high
// The second argument is the integral gain. This is unused for motion profiling
// The third argument is the derivative gain. Tweak this if you are unhappy with the tracking of the trajectory
// The fourth argument is the velocity ratio. This is 1 over the maximum velocity you provided in the 
//      trajectory configuration (it translates m/s to a -1 to 1 scale that your motors can read)
// The fifth argument is your acceleration gain. Tweak this if you want to get to a higher or lower speed quicker
flFollower.configurePIDVA(1.0, 0.0, 0.0, 1 / max_velocity, 0);

Inside your control loop, you can add the following code to calculate the desired output of your motor:

double output = flFollower.calculate(fl_encoder_position);

The above calculate call won't account for the heading of your wheel. If you run this as is, you will be permanently going in a straight line. To fix this, we need to know the heading of your swerve wheel. For most teams, this will be done with an encoder. Some example code for dealing with heading is given below:

double output = flFollower.calculate(fl_encoder_position);
double desiredHeading = Pathfinder.boundHalfDegrees(Pathfinder.r2d(flFollower.getHeading()));    // Bound to -180..180 degrees

frontLeftWheel.setDirection(desiredHeading);
frontLeftWheel.setSpeed(output);

The setDirection implementation is up to you. Usually, for a swerve drive, this will be some kind of PID control loop.

Clone this wiki locally