Following Trajectories (v2)

Now that you have a trajectory generated, this page describes how to follow these trajectories with a real robot to achieve optimal results.

Followers

Generating the trajectory is only half the work. The trajectory contains information about the position, velocity and acceleration of the robot at any given time. So in order to actually drive out a trajectory in real life, we have to somehow make the motors turn an exact amount, at an exact speed and acceleration at any given time. However, most motors and motor controllers are not this simple. It's not as easy as just telling the motor controller to spin the motor at some speed; most of the time you're only able to control the percentage output and direction of the motor.

Fortunately, RobotPathfinder comes with an easy solution to this problem - followers. A follower will take a trajectory, and use the magic of PIDs to control the motors so that they follow the trajectory as best as they can. (If you're not familiar with PIDs, you can learn the basics here.) RobotPathfinder's followers use PDVA controllers (more on that later).

PDVA Control

Instead of using the more common PID control, RobotPathfinder uses the PDVA controller instead. The P and D terms still stand for proportional and derivative. The V and A terms are velocity and acceleration feedforward terms. Basically, they take the desired velocity and acceleration for that moment in time and multiplies them by a constant, then adds it to the total output. Finally, there is an optional Directional-Proportional term. This term takes the difference between the direction the robot is supposed to be facing and the actual direction the robot is facing and multiplies it by a constant, then adds it to the total output.

Using Followers

All followers inherit from the abstract class robot.pathfinder.follower.Follower. Currently, RobotPathfinder only provides one follower, the robot.pathfinder.follower.TankFollower, which takes a TankDriveTrajectory and follows it.

Before creating the TankFollower object, we first have to specify several methods for the follower to set the motor outputs, get distance feedback from the encoders attached to the motors, get times, and so on:

• robot.pathfinder.follower.Follower.Motor is a functional interface that sets a motor's output. There is a single method set(double), with 0 being neutral (not moving), 1 being full speed forwards and -1 being full speed backwards.
• robot.pathfinder.follower.DistanceSource is a functional interface that provides distance feedback. It has a single method getDistance(), which should return the distance a wheel has travelled. The units must be the same as the units used in the trajectory generation.
• robot.pathfinder.follower.TimestampSource is a functional interface that provides time information. There is a single method getTimestamp(), which should return time in seconds.
• robot.pathfinder.follower.DirectionSource is a functional interface that provides direction (heading) information. There is a single method getDirection(), which should return the direction the robot is facing in radians, with counterclockwise turns being positive. This is optional and is only used if you want to use the Directional-Proportional feedback.

The following example is on an FRC robot:

public static final Motor L_MOTOR = Robot.drivetrain::setLeftMotor;
public static final Motor R_MOTOR = Robot.drivetrain::setRightMotor;
public static final DirectionSource GYRO = () -> {
    return Math.toRadians(Robot.drivetrain.getHeading());
};
public static final DistanceSource L_DISTANCE_SOURCE = Robot.drivetrain::getLeftDistance;
public static final DistanceSource R_DISTANCE_SOURCE = Robot.drivetrain::getRightDistance;
public static final TimestampSource TIMESTAMP_SOURCE = Timer::getFPGATimestamp;

Now, we create a follower by passing in the methods, and the PDVA constants:

TankFollower follower = new TankFollower(trajectory, L_MOTOR, R_MOTOR, L_DISTANCE_SOURCE, R_DISTANCE_SOURCE, TIMESTAMP_SOURCE, 
        GYRO, kV, kA, kP, kD, kDP);

Before we start the control loop, the follower needs to be initialized to signal the start of a control loop:

follower.initialize();

Finally, we run the follower in a loop, until the trajectory finishes:

while(follower.isRunning()) {
    follower.run();
    try {
        Thread.sleep(5);
    }
    catch(InterruptedException e) {
        e.printStackTrace();
    }
}

Note that if the follower is interrupted before it could finish, the method robot.pathfinder.follower.TankFollower.stop() must be called to stop the following.

Tuning PDVA Constants

Tuning a control system can be a real pain. Fortunately, here are a few tips to get you started:

• Start tuning by using a straight trajectory. When you're done, move onto a curved trajectory.
• Start with the velocity feedforward term. Getting the value for this term is easy; just set it to be 1.0 divided by the maximum velocity of the wheels. (This is so that when the velocity is at the maximum, the motors also get set to the maximum output.)
• Use the SmartDashboard or some other utility to graph out the errors (you can obtain them by calling the method TankFollower.lastLeftError() and TankFollower.lastRightError()). If the errors are higher when the robot is accelerating/decelerating, increase the acceleration feedforward.
• Adjust the proportional feedback term to reduce the error.
• Adjust the derivative term as needed.

FRC Example

/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package frc.robot.commands;

import com.ctre.phoenix.motorcontrol.NeutralMode;

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Robot;
import frc.robot.RobotMap;
import frc.robot.subsystems.Drivetrain;
import robot.pathfinder.core.TrajectoryParams;
import robot.pathfinder.core.Waypoint;
import robot.pathfinder.core.path.PathType;
import robot.pathfinder.core.trajectory.TankDriveTrajectory;
import robot.pathfinder.follower.Follower.DirectionSource;
import robot.pathfinder.follower.Follower.DistanceSource;
import robot.pathfinder.follower.Follower.Motor;
import robot.pathfinder.follower.Follower.TimestampSource;
import robot.pathfinder.follower.TankFollower;

public class FollowTrajectory extends Command {
    // Change these methods to the ones used by your robot
    public static final Motor L_MOTOR = Robot.drivetrain::setLeftMotor;
    public static final Motor R_MOTOR = Robot.drivetrain::setRightMotor;
    public static final DirectionSource GYRO = () -> {
        return Math.toRadians(Robot.drivetrain.getHeading());
    };
    public static final DistanceSource L_DISTANCE_SOURCE = Robot.drivetrain::getLeftDistance;
    public static final DistanceSource R_DISTANCE_SOURCE = Robot.drivetrain::getRightDistance;
    public static final TimestampSource TIMESTAMP_SOURCE = Timer::getFPGATimestamp;
    
    // Tune these values according to your robot
    public static double kP = 0.025, kD = 0.0005, kV = 0.025, kA = 0.0025, kDP = 0;

    private final TankDriveTrajectory trajectory;
    private TankFollower follower;

    public FollowTrajectory(TankDriveTrajectory trajectory) {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
        requires(Robot.drivetrain);
        this.trajectory = trajectory;
    }

    // Called just before this Command runs the first time
    // Note we made this method public! This is so that Commands that wrap around this one have an easier time.
    @Override
    public void initialize() {
        follower = new TankFollower(trajectory, L_MOTOR, R_MOTOR, L_DISTANCE_SOURCE, R_DISTANCE_SOURCE, TIMESTAMP_SOURCE, 
                GYRO, kV, kA, kP, kD, kDP);
        follower.initialize();
    }

    // Called repeatedly when this Command is scheduled to run
    @Override
    public void execute() {
        follower.run();

        if(Robot.isInDebugMode) {
            SmartDashboard.putNumber("Follower Left Output", follower.lastLeftOutput());
            SmartDashboard.putNumber("Follower Right Output", follower.lastRightOutput());

            SmartDashboard.putNumber("Follower Left Velocity", follower.lastLeftVelocity());
            SmartDashboard.putNumber("Follower Right Velocity", follower.lastRightVelocity());

            SmartDashboard.putNumber("Follower Left Acceleration", follower.lastLeftAcceleration());
            SmartDashboard.putNumber("Follower Right Acceleration", follower.lastRightAcceleration());

            SmartDashboard.putNumber("Follower Left Error", follower.lastLeftError());
            SmartDashboard.putNumber("Follower Right Error", follower.lastRightError());
            
            SmartDashboard.putNumber("Follower Left Error Derivative", follower.lastLeftDerivative());
            SmartDashboard.putNumber("Follower Right Error Derivative", follower.lastRightDerivative());

            SmartDashboard.putNumber("Follower Directional Error", follower.lastDirectionalError());
        }
    }

    // Make this return true when this Command no longer needs to run execute()
    @Override
    public boolean isFinished() {
        return !follower.isRunning();
    }

    // Called once after isFinished returns true
    @Override
    public void end() {
        follower.stop();
        Robot.drivetrain.setMotors(0, 0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    @Override
    public void interrupted() {
        follower.stop();
        Robot.drivetrain.setMotors(0, 0);
    }
}