Add utility classes for easy use in FRC

Pathfinder-FRCSupport/src/main/java/jaci/pathfinder/DriveSubsystem.java

@@ -0,0 +1,9 @@
+package jaci.pathfinder;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+/**
+ * Conveinence class that lets a subsystem act as the PathDriveTrain.
+ */
+public abstract class DriveSubsystem extends Subsystem implements PathDriveTrain {
+}

Pathfinder-FRCSupport/src/main/java/jaci/pathfinder/PathDriveTrain.java

@@ -0,0 +1,32 @@
+package jaci.pathfinder;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+
+/**
+ * PathDriveTrain is an abstraction used to pass odometry in and motor values out
+ */
+public interface PathDriveTrain {
+    public int getLeftEncoderTicks();
+    public int getRightEncoderTicks();
+    public void setMotors(double left, double right);
+    public default double getHeading() {
+        return Double.NaN;
+    }
+    static PathDriveTrain fromParts(Encoder leftEncoder, Encoder rightEncoder, DifferentialDrive drive) {
+        return new PathDriveTrain() {
+            @Override
+            public int getLeftEncoderTicks() {
+                return leftEncoder.get();
+            }
+            @Override
+            public int getRightEncoderTicks() {
+                return rightEncoder.get();
+            }
+            @Override
+            public void setMotors(double left, double right) {
+                drive.tankDrive(left,right);
+            }
+        };
+    }
+}

Pathfinder-FRCSupport/src/main/java/jaci/pathfinder/PathFollower.java

@@ -0,0 +1,60 @@
+package jaci.pathfinder;
+/**
+ * Implement path following logic using PFv1.
+ */
+import jaci.pathfinder.followers.EncoderFollower;
+
+public class PathFollower {
+
+
+    private Trajectory leftTrajectory;
+    private Trajectory rightTrajectory;
+
+    private EncoderFollower leftFollower;
+    private EncoderFollower rightFollower;
+
+    private PathFollowerConfig config;
+
+    private PathDriveTrain driveBase;
+
+    public PathFollower(String csvPath, PathDriveTrain driveBase, PathFollowerConfig config) {
+        leftTrajectory = PathfinderFRC.getTrajectory(csvPath+".left");
+        rightTrajectory = PathfinderFRC.getTrajectory(csvPath+".right");
+        this.config = config;
+        this.driveBase = driveBase;
+        leftFollower = new EncoderFollower();
+        leftFollower.configureEncoder(0, config.encoderCPR, config.wheelDiameter);
+        leftFollower.configurePIDVA(config.kP, config.kI, config.kD, config.kV, config.kA);
+        leftFollower.setTrajectory(leftTrajectory);
+
+        rightFollower = new EncoderFollower();
+        rightFollower.configureEncoder(0, config.encoderCPR, config.wheelDiameter);
+        rightFollower.configurePIDVA(config.kP, config.kI, config.kD, config.kV, config.kA);
+        rightFollower.setTrajectory(rightTrajectory);
+
+    }
+
+    public void run() {
+        double leftOut = leftFollower.calculate(driveBase.getLeftEncoderTicks());
+        double rightOut = rightFollower.calculate(driveBase.getRightEncoderTicks());
+        double gyroHeading = driveBase.getHeading();
+        if(Double.isNaN(gyroHeading)) {
+            driveBase.setMotors(leftOut, rightOut);
+        } else {
+            double angleDiff = Pathfinder.boundHalfDegrees(Pathfinder.r2d(leftFollower.getHeading()) -gyroHeading);
+            driveBase.setMotors(leftOut - config.gyroP * angleDiff, rightOut + config.gyroP * angleDiff);
+        }
+    }
+
+    public boolean isFinished() {
+        return leftFollower.isFinished() || rightFollower.isFinished();
+    }
+
+    public void reset() {
+        leftFollower.reset();
+        rightFollower.reset();
+        leftFollower.configureEncoder(driveBase.getLeftEncoderTicks(),config.encoderCPR,config.wheelDiameter);
+        rightFollower.configureEncoder(driveBase.getRightEncoderTicks(),config.encoderCPR,config.wheelDiameter);
+    }
+
+}

Pathfinder-FRCSupport/src/main/java/jaci/pathfinder/PathFollowerCommand.java

@@ -0,0 +1,30 @@
+package jaci.pathfinder;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * Command class for using a PathFollower in the context of a Command based robot.
+ */
+
+public class PathFollowerCommand extends Command {
+
+    private PathFollower pf;
+    public PathFollowerCommand(String pathName, DriveSubsystem drive, PathFollowerConfig cfg) {
+        requires(drive);
+        pf = new PathFollower(pathName,drive,cfg);
+    }
+
+    @Override
+    protected void initialize() {
+        pf.reset();
+    }
+
+    @Override
+    public void execute() {
+        pf.run();
+    }
+
+    public boolean isFinished() {
+        return pf.isFinished();
+    }
+}

Pathfinder-FRCSupport/src/main/java/jaci/pathfinder/PathFollowerConfig.java

@@ -0,0 +1,65 @@
+package jaci.pathfinder;
+/**
+ * Builder-ish class that holds constants for path following.
+ **/
+public final class PathFollowerConfig {
+    int encoderCPR;
+    double wheelDiameter;
+    double kP;
+    double kI;
+    double kD;
+    double kV;
+    double kA;
+    double gyroP;
+
+    public PathFollowerConfig() {
+    }
+
+    public PathFollowerConfig withEncoderCPR(int encoderCPR) {
+        this.encoderCPR = encoderCPR;
+        return this;
+    }
+
+    public PathFollowerConfig withWheelDiameter(double wheelDiameter) {
+        this.wheelDiameter = wheelDiameter;
+        return this;
+    }
+
+    public PathFollowerConfig withPIDVA(double kP,double kI, double kD, double kV, double kA) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kV = kV;
+        this.kA = kA;
+        return this;
+    }
+
+    public PathFollowerConfig withKP(double kP) {
+        this.kP = kP;
+        return this;
+    }
+
+    public PathFollowerConfig withKI(double kI) {
+        this.kI = kI;
+        return this;
+    }
+
+    public PathFollowerConfig withKD(double kD) {
+        this.kD = kD;
+        return this;
+    }
+
+    public PathFollowerConfig withKV(double kV) {
+        this.kV = kV;
+        return this;
+    }
+
+    public PathFollowerConfig withKA(double kA) {
+        this.kA = kA;
+        return this;
+    }
+    public PathFollowerConfig withgyroP(double gyroP) {
+        this.gyroP = gyroP;
+        return this;
+    }
+}

Pathfinder-FRCSupport/src/main/java/jaci/pathfinder/PathfinderFRC.java

@@ -1,3 +1,5 @@
+package jaci.pathfinder;
+
 import java.io.File;
 import jaci.pathfinder.Trajectory;
 import jaci.pathfinder.Pathfinder;
@@ -23,7 +25,7 @@ private PathfinderFRC() {}
    * Get the absolute path of a trajectory file generated with the given name, usually by PathWeaver.
    * This looks in the deploy directory, e.g. for name "testtraj", "/home/lvuser/deploy/paths/testtraj.pf1.csv"
    * is the result (placed in "src/main/deploy/paths/testtraj.pf1.csv" in your project directory).
-   * 
+   *
    * @param name The name of the path
    * @return The absolute file of the trajectory
    */
@@ -34,12 +36,33 @@ public static File getTrajectoryFile(String name) {
   /**
    * Load a Trajectory from file, at the path described by {@link #getTrajectoryFile(String)}
    * This call is expensive, and as such the result should be stored.
-   * 
+   *
    * @param name The name of the path
    * @return The Trajectory loaded from file.
    */
   public static Trajectory getTrajectory(String name) {
     return Pathfinder.readFromCSV(getTrajectoryFile(name));
   }
 
+  /**
+   * Load a left Trajectory from file, at the path described by {@link #getTrajectoryFile(String)}
+   * This call is expensive, and as such the result should be stored.
+   *
+   * @param name The name of the path
+   * @return The Trajectory loaded from file.
+   */
+  public static Trajectory getLeftTrajectory(String name) {
+    return Pathfinder.readFromCSV(getTrajectoryFile(name + ".left"));
+  }
+
+  /**
+   * Load a right Trajectory from file, at the path described by {@link #getTrajectoryFile(String)}
+   * This call is expensive, and as such the result should be stored.
+   *
+   * @param name The name of the path
+   * @return The Trajectory loaded from file.
+   */
+  public static Trajectory getRightTrajectory(String name) {
+    return Pathfinder.readFromCSV(getTrajectoryFile(name + ".right"));
+  }
 }