HelixFollower

HelixFollower is a path following command used to follow paths generated by BobTrajectory using the FRC Java Command Structure.

Set up dependencies

1. Add the following to your build.gradle, This allows you to pull the dependencies needed from a github repository:

   repositories {
      jcenter()
      maven { url "https://jitpack.io" }
   }

2. Add these lines to the dependencies block:

    compile 'com.github.TripleHelixProgramming:HelixUtilities:v2020.1'
    compile 'com.github.Team319:BobTrajectory:v2.0.0'

Integrating into your robot

To integrate the path follower into our robot you will need to create a class that extends HelixFollower and implement the required methods.

public class PathFollower extends HelixFollower {

    private Drivetrain drivetrain = Drivetrain.getDrivetrain();

    // These are the 2 PID controllers that will handle error for your total travel distance and heading
    private PIDController headingController = new PIDController(15, 0, 0, 0.001);
    private PIDController distanceController = new PIDController(10, 0, 0, 0.001);

    public PathFollower(Path path) {
        super(path);
        // Make sure to require your subsystem so you don't have conflicting commands
        requires(drivetrain);
    }

    @Override
    public void resetDistance() {
        // We need to reset the encoders back to 0 at the start of the path
        drivetrain.resetEncoders();
    }

    @Override
    public PIDController getHeadingController() {
        // Here we return the PID controller that we're using to correct the heading error through the path
        return headingController;
    }

    @Override
    public PIDController getDistanceController() {
        // Here we return the PID controller that we're using to correct the distance error through the path
        return distanceController;
    }

    @Override
    public double getCurrentDistance() {
        // Here we need to return the overall robot distance traveled in FEET in this example we are averaging 
        // the two sides of the drivetrain to give is the robot's distance travelled
        return (drivetrain.getLeftPosition() + drivetrain.getRightPosition()) / 2.0;
    }

    @Override
    public double getCurrentHeading() {
        // Here we need to return the current heading of the robot in RADIANS (positive counter-clockwise).
        return Math.toRadians(drivetrain.getHeading());
    }

    @Override
    public void useOutputs(double left, double right) {
        // Here we will use the provided parameters in FPS and send them off to our drivetrain. In this example 
        // the max velocity of our drivetrain is 12 FPS. We are dividing the two provided parameters by the max 
        // veocity to convert them into a percentage and sending them off to our drivetrain.
        drivetrain.setRawPercentOutput(left/12.0, right/12.0);
    }
}

Running paths

To run a path you'll create a new PathFollower instance and pass in the desired path to run.

autonomousCommand = new PathFollower(new RightTurn());

Tuning

For tuning you'll need 2 paths, one that drives straight and one that turns. We like to do something like 4 feet forward for one and a 89.99 degree turn that goes 3 feet forward and 3 feet to the right for the second.

The first thing you'll tune is the distance PID controller by running the straight path that you generated.

1. The first time you run the path the robot is going to move, even with all 0's for the PID gains. The reason for this is because HelixFollower is going to pass back the uncorrected/expected velocity outputs for each trajectory point.
2. Once you verify that your robot moves you'll want to start adding in a P gain to the distanceController PIDController (first parameter).
3. Keep playing with this value until you're happy with the robot consistently going the same distance. HelixFollower automatically pushes a "Distance Path Error" value to the SmartDashboard that you can use to track the error of the travel distance.
4. If your robot is consistently going the same distance, but not the correct distance you'll need to adjust your conversion from encoder units to feet. The robot thinks it is going the correct distance but the logical to the physical relationship is off.

Next we'll tune the heading PID controller by running the turning path that you generated.

1. The first time you run the path the robot should attempt to turn, but it likely will not turn enough.
2. First you'll want to adjust the wheel base setting in your BobTrajectory configuration, by increasing the wheel base you'll make the robot turn more and by decreasing it you'll make the robot turn less. Adjust this value and regenerate your paths and rerun them until your robot seems to be semi doing what it should.
3. Next you'll want to start adding in some P gain to your headingController PIDController (first parameter).
4. Keep playing with this value until you're happy with the robot consistently turning the same amount. HelixFollower automatically pushes a "Heading Path Error" value to the SmartDashboard that you can use to track the error of the heading.

Running your path in different directions

HelixFollower provides the ability to mirror (flip the left and right direction) and reverse (robot drives the path but with the robot backwards) your path.

Mirror

autonomousCommand = new PathFollower(new RightTurn()).mirror();

Reverse

autonomousCommand = new PathFollower(new RightTurn()).reverse();

Both

autonomousCommand = new PathFollower(new RightTurn()).mirror().reverse();