Pathfinder basic

src/main/java/org/usfirst/frc4904/robot/RobotMap.java

@@ -54,6 +54,8 @@ public static class Wheel {
 			public static final double INCHES_PER_TICK = Metrics.Wheel.CIRCUMFERENCE_INCHES
 				/ Metrics.Wheel.TICKS_PER_REVOLUTION;
 		}
+		public static final double ROBOT_WIDTH_INCHES = -1; // TODO: set
+		public static final double ROBOT_WIDTH_METERS = ROBOT_WIDTH_INCHES * 0.0254; // TODO: set
 	}
 
 	public static class PID {

src/main/java/org/usfirst/frc4904/robot/autonly/PathfinderBasic.java

@@ -0,0 +1,69 @@
+package org.usfirst.frc4904.robot.autonly;
+
+
+import org.usfirst.frc4904.robot.RobotMap;
+import edu.wpi.first.wpilibj.command.Command;
+import jaci.pathfinder.Pathfinder;
+import jaci.pathfinder.Trajectory;
+import jaci.pathfinder.Waypoint;
+import jaci.pathfinder.followers.DistanceFollower;
+import jaci.pathfinder.modifiers.TankModifier;
+
+public class PathfinderBasic extends Command {
+    double leftDistanceOffset = RobotMap.Component.leftWheelEncoder.getDistance();
+    double rightDistanceOffset = RobotMap.Component.rightWheelEncoder.getDistance();
+    Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, Trajectory.Config.SAMPLES_HIGH,
+        0.05, 1.7, 2.0, 60.0);
+    Trajectory trajectory;
+    TankModifier modifier;
+    DistanceFollower leftTrajectory;
+    DistanceFollower rightTrajectory;
+
+    public PathfinderBasic(Waypoint... waypoints) {
+        requires(RobotMap.Component.leftWheelA);
+        requires(RobotMap.Component.leftWheelB);
+        requires(RobotMap.Component.rightWheelA);
+        requires(RobotMap.Component.rightWheelB);
+        trajectory = Pathfinder.generate(waypoints, config);
+        modifier = new TankModifier(trajectory).modify(RobotMap.Metrics.ROBOT_WIDTH_METERS);
+        leftTrajectory = new DistanceFollower(modifier.getLeftTrajectory());
+        rightTrajectory = new DistanceFollower(modifier.getRightTrajectory());
+        leftTrajectory.configurePIDVA(0.0, 0.0, 0.0, 0.0, 0.0); // TODO: set these PIDVA values
+        rightTrajectory.configurePIDVA(0.0, 0.0, 0.0, 0.0, 0.0);
+    }
+
+    @Override
+    protected void initialize() {
+        leftDistanceOffset = RobotMap.Component.leftWheelEncoder.getDistance();
+        rightDistanceOffset = RobotMap.Component.rightWheelEncoder.getDistance();
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    @Override
+    protected void execute() {
+        double leftOutput = leftTrajectory.calculate(RobotMap.Component.leftWheelEncoder.getDistance() - leftDistanceOffset);
+        double rightOutput = rightTrajectory
+            .calculate(RobotMap.Component.rightWheelEncoder.getDistance() - rightDistanceOffset);
+        RobotMap.Component.leftWheelA.set(leftOutput);
+        RobotMap.Component.leftWheelB.set(leftOutput);
+        RobotMap.Component.rightWheelA.set(rightOutput);
+        RobotMap.Component.rightWheelB.set(rightOutput);
+    }
+
+    // Make this return true when this Command no longer needs to run execute()
+    @Override
+    protected boolean isFinished() {
+        return leftTrajectory.isFinished() && rightTrajectory.isFinished();
+    }
+
+    // Called once after isFinished returns true
+    @Override
+    protected void end() {
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    @Override
+    protected void interrupted() {
+    }
+}

vendordeps/PathfinderOLD.json

@@ -0,0 +1,85 @@
+{
+    "fileName": "PathfinderOLD.json",
+    "name": "PathfinderOld",
+    "version": "2019.2.19",
+    "uuid": "7194a2d4-2860-4bcc-86c0-97879737d875",
+    "mavenUrls": [
+        "https://dev.imjac.in/maven"
+    ],
+    "jsonUrl": "https://dev.imjac.in/maven/jaci/pathfinder/PathfinderOLD-latest.json",
+    "cppDependencies": [
+        {
+            "groupId": "jaci.pathfinder",
+            "artifactId": "Pathfinder-Core",
+            "version": "2019.2.19",
+            "libName": "pathfinder",
+            "configuration": "native_pathfinder_old",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxx86-64",
+                "windowsx86-64",
+                "osxx86-64",
+                "linuxathena",
+                "linuxraspbian"
+            ]
+        },
+        {
+            "groupId": "jaci.pathfinder",
+            "artifactId": "Pathfinder-FRCSupport",
+            "version": "2019.2.19",
+            "libName": "pathfinder_frc",
+            "configuration": "native_pathfinder_old",
+            "headerClassifier": "headers",
+            "binaryPlatforms": []
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "jaci.pathfinder",
+            "artifactId": "Pathfinder-Java",
+            "version": "2019.2.19"
+        },
+        {
+            "groupId": "jaci.jniloader",
+            "artifactId": "JNILoader",
+            "version": "1.0.1"
+        },
+        {
+            "groupId": "jaci.pathfinder",
+            "artifactId": "Pathfinder-FRCSupport",
+            "version": "2019.2.19"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "jaci.pathfinder",
+            "artifactId": "Pathfinder-JNI",
+            "version": "2019.2.19",
+            "isJar": true,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "linuxx86-64",
+                "windowsx86-64",
+                "osxx86-64",
+                "linuxathena",
+                "linuxraspbian"
+            ]
+        },
+        {
+            "groupId": "jaci.pathfinder",
+            "artifactId": "Pathfinder-CoreJNI",
+            "version": "2019.2.19",
+            "isJar": true,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "linuxx86-64",
+                "windowsx86-64",
+                "osxx86-64",
+                "linuxathena",
+                "linuxraspbian"
+            ]
+        }
+    ]
+}