diff --git a/Pathfinder-FRCSupport/src/main/java/jaci/pathfinder/DriveSubsystem.java b/Pathfinder-FRCSupport/src/main/java/jaci/pathfinder/DriveSubsystem.java new file mode 100644 index 0000000..b866c28 --- /dev/null +++ b/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 { +} \ No newline at end of file diff --git a/Pathfinder-FRCSupport/src/main/java/jaci/pathfinder/PathDriveTrain.java b/Pathfinder-FRCSupport/src/main/java/jaci/pathfinder/PathDriveTrain.java new file mode 100644 index 0000000..54e04f1 --- /dev/null +++ b/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); + } + }; + } +} \ No newline at end of file diff --git a/Pathfinder-FRCSupport/src/main/java/jaci/pathfinder/PathFollower.java b/Pathfinder-FRCSupport/src/main/java/jaci/pathfinder/PathFollower.java new file mode 100644 index 0000000..ad2e916 --- /dev/null +++ b/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); + } + +} \ No newline at end of file diff --git a/Pathfinder-FRCSupport/src/main/java/jaci/pathfinder/PathFollowerCommand.java b/Pathfinder-FRCSupport/src/main/java/jaci/pathfinder/PathFollowerCommand.java new file mode 100644 index 0000000..38653b2 --- /dev/null +++ b/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(); + } +} \ No newline at end of file diff --git a/Pathfinder-FRCSupport/src/main/java/jaci/pathfinder/PathFollowerConfig.java b/Pathfinder-FRCSupport/src/main/java/jaci/pathfinder/PathFollowerConfig.java new file mode 100644 index 0000000..9c14ffe --- /dev/null +++ b/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; + } +} \ No newline at end of file diff --git a/Pathfinder-FRCSupport/src/main/java/jaci/pathfinder/PathfinderFRC.java b/Pathfinder-FRCSupport/src/main/java/jaci/pathfinder/PathfinderFRC.java index cdd504f..4dd287a 100644 --- a/Pathfinder-FRCSupport/src/main/java/jaci/pathfinder/PathfinderFRC.java +++ b/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,7 +36,7 @@ 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. */ @@ -42,4 +44,25 @@ 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")); + } } \ No newline at end of file