Pathfinder for FRC CPP

This is the wiki entry for how to install and use Pathfinder for an FRC C++ Project.

Downloading Pathfinder

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

Using the Library

Because Pathfinder is written in C and not C++, all of the functions that pathfinder provide are global and not namespaced. Thankfully, everything is prefixed with pathfinder_, so they shouldn't conflict with any existing functions (except for pathfinder's math functions and macros, like d2r, r2d and PI, TAU, MIN and MAX)

First, include the library in your program:

#include <pathfinder.h>

Whenever you want to generate a trajectory, you can choose the Waypoints and your Robot's Configuration (max speed, acceleration, etc).

int POINT_LENGTH = 3;

Waypoint points[POINT_LENGTH];

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

TrajectoryCandidate candidate;

// Prepare the Trajectory for Generation.
//
// Arguments: 
// Fit Function:        FIT_HERMITE_CUBIC or FIT_HERMITE_QUINTIC
// Sample Count:        PATHFINDER_SAMPLES_HIGH (100 000)
//                      PATHFINDER_SAMPLES_LOW  (10 000)
//                      PATHFINDER_SAMPLES_FAST (1 000)
// Time Step:           0.001 Seconds
// Max Velocity:        15 m/s
// Max Acceleration:    10 m/s/s
// Max Jerk:            60 m/s/s/s
pathfinder_prepare(points, POINT_LENGTH, FIT_HERMITE_CUBIC, PATHFINDER_SAMPLES_HIGH, 0.001, 15.0, 10.0, 60.0, &candidate);

int length = candidate.length;

// Array of Segments (the trajectory points) to store the trajectory in
Segment *trajectory = malloc(length * sizeof(Segment));

// Generate the trajectory
pathfinder_generate(&candidate, trajectory);

Thankfully, you only have to do that once per generation. It's recommended to put this inside of it's own function if you plan on generating multiple profiles.

You can also modify the Trajectory for either Tank or Swerve drive!

Segment leftTrajectory[length];
Segment rightTrajectory[length];

// The distance between the left and right sides of the wheelbase is 0.6m
double wheelbase_width = 0.6;

// Generate the Left and Right trajectories of the wheelbase using the 
// originally generated trajectory
pathfinder_modify_tank(trajectory, length, leftTrajectory, rightTrajectory, wheelbase_width);

// Output Trajectories
Segment *frontLeft = malloc(length * sizeof(Segment));
Segment *frontRight = malloc(length * sizeof(Segment));
Segment *backLeft = malloc(length * sizeof(Segment));
Segment *backRight = malloc(length * sizeof(Segment));
// The distance between the left and right sides of the wheelbase is 0.6m
double wheelbase_width = 0.6;

// The distance between the front and back sides of the wheelbase is 0.5m
double wheelbase_depth = 0.5;

// The swerve mode to generate will be the 'default' mode, where the robot
// will constantly be facing forward and 'sliding' sideways to follow a
// curved path.
SWERVE_MODE mode = SWERVE_DEFAULT;

// Generate the trajectories of each of the 4 swerve wheels using the 
// originally generated trajectory
pathfinder_modify_swerve(trajectory, length, frontLeft, frontRight, 
        backLeft, backRight, wheelbase_width, wheelbase_depth, mode);

Constants such as Wheelbase Width and Depth should be stored inside of your RobotMap or similar class.

Following a Trajectory

To get your robot to follow a trajectory, you can use the pathfinder_follow_encoder function. Before you use this function, there is some setup that needs to occur.

First, you need to setup the structure to hold the data about your current following status, such as cumulative error and what segment it is working on. This is a simple malloc call. Put this somewhere in the class that is dealing with your trajectory following, and don't forget to free it when you are done with it!

EncoderFollower follower = malloc(sizeof(EncoderFollower));
follower.last_error = 0; follower.segment = 0; follower.finished = 0;     // Just in case!

Now we can setup the configuration for your encoders. This is going to contain the initial offset and the ticks per revolution of your encoders. It will also contain your Robot's Wheel (or pulley for track systems) Circumference (in meters), and the PID/VA coefficients. You should do this just before you start following a trajectory in order to get the encoder's offset correct.

EncoderConfig config = { encoder_position, 1000, wheel_circumference,      // Position, Ticks per Rev, Wheel Circumference
                         1.0, 0.0, 0.0, 1.0 / max_velocity, 0.0};          // Kp, Ki, Kd and Kv, Ka

Usually, your Kp will be fairly high (0.8 - 1.2 is fairly common).
The Ki value is unused currently.
The Kd value should be adjusted if you are unhappy with the tracking.
The Kv value is 1 over your robot's maximum velocity as provided in the initial trajectory configuration. This translates meters per second to a -1..1 scale your motor controllers can read.
The Ka value should be adjusted if you want to tweak the acceleration rate of your robot.

Tank Drive

For tank drive systems, you must use pathfinder_follow_encoder twice, once for each side. The value returned will be the desired output for your Motor Controllers on each side.

// Arg 1: The EncoderConfig
// Arg 2: The EncoderFollower for this side
// Arg 3: The Trajectory generated from `pathfinder_modify_tank`
// Arg 4: The Length of the Trajectory (length used in Segment seg[length];)
// Arg 5: The current value of your encoder
double l = pathfinder_follow_encoder(leftConfig, &leftFollower, leftTrajectory, trajectory_length, l_encoder_value);
double r = pathfinder_follow_encoder(rightConfig, &rightFollower, rightTrajectory, trajectory_length, r_encoder_value);

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.

// -- using l and r from the previous code block -- //
double gyro_heading = ... your gyro code here ...      // Assuming gyro angle is given in degrees
double desired_heading = r2d(leftFollower.heading);

double angle_difference = desired_heading - gyro_heading;    // Make sure to bound this from -180 to 180, otherwise you will get super large values

// This allows the angle distance to respect 'wrapping', where 360 and 0 are the same value.
angle_difference = std::fmod(angle_difference, 360.0);
if (std::abs(angle_difference) > 180.0) {
  angle_difference = (angle_difference > 0) ? angle_difference - 360 : angle_difference + 360;
} 

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

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

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.

// Arg 1: The EncoderConfig
// Arg 2: The EncoderFollower for this side
// Arg 3: The Trajectory generated from `pathfinder_modify_swerve`
// Arg 4: The Length of the Trajectory (length used in Segment seg[length];)
// Arg 5: The current value of your encoder
double fl = pathfinder_follow_encoder(frontLeftConfig, &frontLeftFollower, frontLeftTrajectory, trajectory_length, fl_encoder_value);

That gives us the magnitude to spin our wheels at, which can be passed directly to the motor controller for the driving part. For direction, we can use the heading property of the EncoderFollower struct.

double desired_heading = r2d(frontLeftFollower.heading);

frontLeftWheel.setDirection(desired_heading);
frontLeftWheel.setSpeed(fl);

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

NOTE: When you're done, don't forget to free() any malloc()'d pointers