evertything has been redone

2019robot/src/main/java/org/usfirst/frc4388/paths/PathBuilder.java

@@ -0,0 +1,155 @@
+package org.usfirst.frc4388.paths;
+
+import java.util.List;
+
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.control.PathSegment;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+/**
+ * Class used to convert a list of Waypoints into a Path object consisting of arc and line PathSegments
+ * 
+ * @see Waypoint
+ * @see Path
+ * @see PathSegment
+ */
+public class PathBuilder {
+    private static final double kEpsilon = 1E-9;
+    private static final double kReallyBigNumber = 1E9;
+
+    public static Path buildPathFromWaypoints(List<Waypoint> w) {
+        Path p = new Path();
+        if (w.size() < 2)
+            throw new Error("Path must contain at least 2 waypoints");
+        int i = 0;
+        if (w.size() > 2) {
+            do {
+                new Arc(getPoint(w, i), getPoint(w, i + 1), getPoint(w, i + 2)).addToPath(p);
+                i++;
+            } while (i < w.size() - 2);
+        }
+        new Line(w.get(w.size() - 2), w.get(w.size() - 1)).addToPath(p, 0);
+        p.extrapolateLast();
+        p.verifySpeeds();
+        // System.out.println(p);
+        return p;
+    }
+
+    private static Waypoint getPoint(List<Waypoint> w, int i) {
+        if (i > w.size())
+            return w.get(w.size() - 1);
+        return w.get(i);
+    }
+
+    /**
+     * A waypoint along a path. Contains a position, radius (for creating curved paths), and speed. The information from
+     * these waypoints is used by the PathBuilder class to generate Paths. Waypoints also contain an optional marker
+     * that is used by the WaitForPathMarkerAction.
+     *
+     * @see PathBuilder
+     * @see WaitForPathMarkerAction
+     */
+    public static class Waypoint {
+        Translation2d position;
+        double radius;
+        double speed;
+        String marker;
+
+        public Waypoint(Waypoint other) {
+            this(other.position.x(), other.position.y(), other.radius, other.speed, other.marker);
+        }
+
+        public Waypoint(double x, double y, double r, double s) {
+            position = new Translation2d(x, y);
+            radius = r;
+            speed = s;
+        }
+
+        public Waypoint(Translation2d pos, double r, double s) {
+            position = pos;
+            radius = r;
+            speed = s;
+        }
+
+        public Waypoint(double x, double y, double r, double s, String m) {
+            position = new Translation2d(x, y);
+            radius = r;
+            speed = s;
+            marker = m;
+        }
+    }
+
+    /**
+     * A Line object is formed by two Waypoints. Contains a start and end position, slope, and speed.
+     */
+    static class Line {
+        Waypoint a;
+        Waypoint b;
+        Translation2d start;
+        Translation2d end;
+        Translation2d slope;
+        double speed;
+
+        public Line(Waypoint a, Waypoint b) {
+            this.a = a;
+            this.b = b;
+            slope = new Translation2d(a.position, b.position);
+            speed = b.speed;
+            start = a.position.translateBy(slope.scale(a.radius / slope.norm()));
+            end = b.position.translateBy(slope.scale(-b.radius / slope.norm()));
+        }
+
+        private void addToPath(Path p, double endSpeed) {
+            double pathLength = new Translation2d(end, start).norm();
+            if (pathLength > kEpsilon) {
+                if (b.marker != null) {
+                    p.addSegment(new PathSegment(start.x(), start.y(), end.x(), end.y(), b.speed,
+                            p.getLastMotionState(), endSpeed, b.marker));
+                } else {
+                    p.addSegment(new PathSegment(start.x(), start.y(), end.x(), end.y(), b.speed,
+                            p.getLastMotionState(), endSpeed));
+                }
+            }
+
+        }
+    }
+
+    /**
+     * An Arc object is formed by two Lines that share a common Waypoint. Contains a center position, radius, and speed.
+     */
+    static class Arc {
+        Line a;
+        Line b;
+        Translation2d center;
+        double radius;
+        double speed;
+
+        public Arc(Waypoint a, Waypoint b, Waypoint c) {
+            this(new Line(a, b), new Line(b, c));
+        }
+
+        public Arc(Line a, Line b) {
+            this.a = a;
+            this.b = b;
+            this.speed = (a.speed + b.speed) / 2;
+            this.center = intersect(a, b);
+            this.radius = new Translation2d(center, a.end).norm();
+        }
+
+        private void addToPath(Path p) {
+            a.addToPath(p, speed);
+            if (radius > kEpsilon && radius < kReallyBigNumber) {
+                p.addSegment(new PathSegment(a.end.x(), a.end.y(), b.start.x(), b.start.y(), center.x(), center.y(),
+                        speed, p.getLastMotionState(), b.speed));
+            }
+        }
+
+        private static Translation2d intersect(Line l1, Line l2) {
+            final RigidTransform2d lineA = new RigidTransform2d(l1.end, new Rotation2d(l1.slope, true).normal());
+            final RigidTransform2d lineB = new RigidTransform2d(l2.start, new Rotation2d(l2.slope, true).normal());
+            return lineA.intersection(lineB);
+        }
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/PathContainer.java

@@ -0,0 +1,16 @@
+package org.usfirst.frc4388.paths;
+
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+
+/**
+ * Interface containing all information necessary for a path including the Path itself, the Path's starting pose, and
+ * whether or not the robot should drive in reverse along the path.
+ */
+public interface PathContainer {
+    Path buildPath();
+
+    RigidTransform2d getStartPose();
+
+    boolean isReversed();
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/Backup5.java

@@ -0,0 +1,34 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class Backup5 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(218,236,0,60));
+        sWaypoints.add(new Waypoint(223,236,0,40));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(218, 236), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/Backup5Right.java

@@ -0,0 +1,34 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class Backup5Right implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(218,90,0,60));
+        sWaypoints.add(new Waypoint(223,89,0,40));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(218, 90), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/CenterStartToScaleLeft.java

@@ -0,0 +1,38 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class CenterStartToScaleLeft implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(19,157,0,0));
+        sWaypoints.add(new Waypoint(40,157,20,75));
+        sWaypoints.add(new Waypoint(115,282,60,75));
+        sWaypoints.add(new Waypoint(210,239,20,75,  "raiseElevator"));
+        sWaypoints.add(new Waypoint(250,213,20,75));
+        sWaypoints.add(new Waypoint(272,210,0,75));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(19, 157), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/CenterStartToScaleRight.java

@@ -0,0 +1,38 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class CenterStartToScaleRight implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(19,157,0,0));
+        sWaypoints.add(new Waypoint(40,157,20,75));
+        sWaypoints.add(new Waypoint(115,46,60,75));
+        sWaypoints.add(new Waypoint(210,81,0,75,  "raiseElevator"));
+        sWaypoints.add(new Waypoint(250,106,20,75));
+        sWaypoints.add(new Waypoint(275,116,0,75));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(19, 157), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/CenterStartToSwitchLeft.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class CenterStartToSwitchLeft implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(19,157,0,80));
+        sWaypoints.add(new Waypoint(40,157,20,80));
+        sWaypoints.add(new Waypoint(100,209,15,80));
+        sWaypoints.add(new Waypoint(116,214,0,80));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(19, 157), Rotation2d.fromDegrees(0.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/CenterStartToSwitchLeftV2.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class CenterStartToSwitchLeftV2 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(19,157,0,80));
+        sWaypoints.add(new Waypoint(40,157,20,80));
+        sWaypoints.add(new Waypoint(100,204,15,80));
+        sWaypoints.add(new Waypoint(116,209,0,80));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(19, 157), Rotation2d.fromDegrees(0.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/CenterStartToSwitchRight.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class CenterStartToSwitchRight implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(19,157,0,80));
+        sWaypoints.add(new Waypoint(40,157,20,80));
+        sWaypoints.add(new Waypoint(100,114,15,80));
+        sWaypoints.add(new Waypoint(121,113,0,80));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(19, 157), Rotation2d.fromDegrees(0.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/Forward5.java

@@ -0,0 +1,34 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class Forward5 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(230,236,0,60));
+        sWaypoints.add(new Waypoint(218,234,0,40));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(230, 236), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/Forward5Right.java

@@ -0,0 +1,34 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class Forward5Right implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(230,89,0,60));
+        sWaypoints.add(new Waypoint(218,91,0,40));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(230, 89), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/GoStraight.java

@@ -0,0 +1,34 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class GoStraight implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(18,70,0,0));
+        sWaypoints.add(new Waypoint(150,70,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(18, 70), Rotation2d.fromDegrees(180)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToCenterStart.java

@@ -0,0 +1,38 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class LeftStartToCenterStart implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(18,274,0,40));
+        sWaypoints.add(new Waypoint(39,274,20,40));
+        sWaypoints.add(new Waypoint(60,250,0,40));
+        sWaypoints.add(new Waypoint(80,170,20,40));
+        sWaypoints.add(new Waypoint(45,160,0,40));
+        sWaypoints.add(new Waypoint(20,160,0,40));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(18, 274), Rotation2d.fromDegrees(180)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleLeft.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class LeftStartToScaleLeft implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(18,274,0,100));
+        sWaypoints.add(new Waypoint(180,274,30,100,     "raiseElevator"));
+        sWaypoints.add(new Waypoint(240,239,30,100));
+        sWaypoints.add(new Waypoint(273,241,0,100));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(18, 274), Rotation2d.fromDegrees(180)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleLeftV2.java

@@ -0,0 +1,37 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class LeftStartToScaleLeftV2 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(18,274,0,130));
+        sWaypoints.add(new Waypoint(40,274,0,130,     "shiftHi"));
+        sWaypoints.add(new Waypoint(145,273,55,130,                "raiseElevator"));
+        sWaypoints.add(new Waypoint(270,232,0,130,           "startEject"));
+        sWaypoints.add(new Waypoint(287,225,0,130));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(18, 274), Rotation2d.fromDegrees(180)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleRight.java

@@ -0,0 +1,38 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class LeftStartToScaleRight implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(18,274,0,110));
+        sWaypoints.add(new Waypoint(159,274,0,110));
+        sWaypoints.add(new Waypoint(225,274,40,110));
+        sWaypoints.add(new Waypoint(220,87,30,110));
+        sWaypoints.add(new Waypoint(250,87,0,50,   "raiseElevator"));
+        sWaypoints.add(new Waypoint(275,90,0,40));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(18, 274), Rotation2d.fromDegrees(180)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleRightAPR.java

@@ -0,0 +1,38 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class LeftStartToScaleRightAPR implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(18,274,0,40));
+        sWaypoints.add(new Waypoint(55,274,20,40));
+        sWaypoints.add(new Waypoint(55,65,80,80));
+        sWaypoints.add(new Waypoint(260,55,0,80));
+        sWaypoints.add(new Waypoint(310,55,20,60));
+        sWaypoints.add(new Waypoint(320,20,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(18, 274), Rotation2d.fromDegrees(180)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleRightAPRV2.java

@@ -0,0 +1,42 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class LeftStartToScaleRightAPRV2 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(18,274,0,40));
+        sWaypoints.add(new Waypoint(55,274,20,40));
+        sWaypoints.add(new Waypoint(55,250,0,110,    "shiftHi"));
+        sWaypoints.add(new Waypoint(55,210,0,110));
+        sWaypoints.add(new Waypoint(55,105,80,110));
+        sWaypoints.add(new Waypoint(140,100,0,80));
+        sWaypoints.add(new Waypoint(270,115,0,80,  "shiftLow"));
+        sWaypoints.add(new Waypoint(300,120,0,80, "raiseElevator"));
+        sWaypoints.add(new Waypoint(330,120,20,80));
+        sWaypoints.add(new Waypoint(330,100,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(18, 274), Rotation2d.fromDegrees(180)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleRightAPRV3.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class LeftStartToScaleRightAPRV3 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(18,274,0,40));
+        sWaypoints.add(new Waypoint(55,274,20,40));
+        sWaypoints.add(new Waypoint(55,35,80,100));
+        sWaypoints.add(new Waypoint(313,25,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(18, 274), Rotation2d.fromDegrees(180)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToScaleRightSave.java

@@ -0,0 +1,42 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class LeftStartToScaleRightSave implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(18,274,0,110));
+        sWaypoints.add(new Waypoint(159,274,0,110));
+        sWaypoints.add(new Waypoint(220,274,40,110));
+        sWaypoints.add(new Waypoint(214,210,0,60));
+        sWaypoints.add(new Waypoint(210,180,0,100));
+        sWaypoints.add(new Waypoint(209,150,0,80));
+        sWaypoints.add(new Waypoint(209,120,0,70));
+        sWaypoints.add(new Waypoint(214,87,30,50));
+        sWaypoints.add(new Waypoint(250,87,0,50,   "raiseElevator"));
+        sWaypoints.add(new Waypoint(270,90,0,40));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(18, 274), Rotation2d.fromDegrees(180)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToSwitchLeftV3.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class LeftStartToSwitchLeftV3 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(65,270,0,60));
+        sWaypoints.add(new Waypoint(65,231,30,60, "raiseElevator"));
+        sWaypoints.add(new Waypoint(110,231,0,60));
+        sWaypoints.add(new Waypoint(126,231,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(65, 270), Rotation2d.fromDegrees(-90.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToSwitchRight.java

@@ -0,0 +1,38 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class LeftStartToSwitchRight implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(18,274,0,40));
+        sWaypoints.add(new Waypoint(39,274,20,40));
+        sWaypoints.add(new Waypoint(60,200,0,40));
+        sWaypoints.add(new Waypoint(80,129,20,40));
+        sWaypoints.add(new Waypoint(106,124,0,40));
+        sWaypoints.add(new Waypoint(126,124,0,40));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(18, 274), Rotation2d.fromDegrees(0.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftStartToSwitchRightV3.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class LeftStartToSwitchRightV3 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(65,270,0,80));
+        sWaypoints.add(new Waypoint(65,134,40,80,  "raiseElevator"));
+        sWaypoints.add(new Waypoint(110,134,0,80));
+        sWaypoints.add(new Waypoint(126,134,0,80));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(65, 270), Rotation2d.fromDegrees(-90.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftSwitch2ndCube.java

@@ -0,0 +1,34 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class LeftSwitch2ndCube implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(230,236,0,30));
+        sWaypoints.add(new Waypoint(195,215,0,30));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(230, 236), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/LeftSwitch2ndCubeV2.java

@@ -0,0 +1,35 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class LeftSwitch2ndCubeV2 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(230,236,0,60));
+        sWaypoints.add(new Waypoint(210,220,0,40));
+        sWaypoints.add(new Waypoint(201,205,0,20));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(230, 236), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/PyramidToScaleRight.java

@@ -0,0 +1,37 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class PyramidToScaleRight implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(100,165,0,60));
+        sWaypoints.add(new Waypoint(70,165,20,60));
+        sWaypoints.add(new Waypoint(80,72,45,60,  "shiftHi"));
+        sWaypoints.add(new Waypoint(230,50,0,130,  "raiseElevator"));
+        sWaypoints.add(new Waypoint(400,40,0,130));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(100, 165), Rotation2d.fromDegrees(0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/PyramidToSwitchLeft.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class PyramidToSwitchLeft implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(46,179,0,60));
+        sWaypoints.add(new Waypoint(66,179,15,60));
+        sWaypoints.add(new Waypoint(104,231,10,60));
+        sWaypoints.add(new Waypoint(116,236,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(46, 179), Rotation2d.fromDegrees(0.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/PyramidToSwitchLeftV2.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class PyramidToSwitchLeftV2 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(46,179,0,60));
+        sWaypoints.add(new Waypoint(66,184,15,60));
+        sWaypoints.add(new Waypoint(94,211,10,60));
+        sWaypoints.add(new Waypoint(120,216,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(46, 179), Rotation2d.fromDegrees(0.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/PyramidToSwitchRight.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class PyramidToSwitchRight implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(46,145,0,60));
+        sWaypoints.add(new Waypoint(66,145,15,60));
+        sWaypoints.add(new Waypoint(110,93,10,60));
+        sWaypoints.add(new Waypoint(122,88,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(46, 145), Rotation2d.fromDegrees(0.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/PyramidToSwitchRightV2.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class PyramidToSwitchRightV2 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(46,145,0,80));
+        sWaypoints.add(new Waypoint(66,145,15,80));
+        sWaypoints.add(new Waypoint(100,93,15,80));
+        sWaypoints.add(new Waypoint(118,88,0,50));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(46, 145), Rotation2d.fromDegrees(0.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeft.java

@@ -0,0 +1,38 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class RightStartToScaleLeft implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(18,50,0,0));
+        sWaypoints.add(new Waypoint(220,50,40,60));
+        sWaypoints.add(new Waypoint(214,114,0,60,    "raiseElevator"));
+        sWaypoints.add(new Waypoint(204,174,0,60));
+        sWaypoints.add(new Waypoint(204,232,30,60));
+        sWaypoints.add(new Waypoint(275,237,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(18, 50), Rotation2d.fromDegrees(180)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeftAPR.java

@@ -0,0 +1,38 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class RightStartToScaleLeftAPR implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(18,51,0,40));
+        sWaypoints.add(new Waypoint(55,51,20,40));
+        sWaypoints.add(new Waypoint(55,260,80,100));
+        sWaypoints.add(new Waypoint(260,270,0,100));
+        sWaypoints.add(new Waypoint(310,270,20,60));
+        sWaypoints.add(new Waypoint(320,295,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(18, 51), Rotation2d.fromDegrees(180)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeftAPRV2.java

@@ -0,0 +1,42 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class RightStartToScaleLeftAPRV2 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(18,51,0,40));
+        sWaypoints.add(new Waypoint(55,51,20,40));
+        sWaypoints.add(new Waypoint(55,75,0,110,      "shiftHi"));
+        sWaypoints.add(new Waypoint(55,130,0,110));
+        sWaypoints.add(new Waypoint(55,230,80,110));
+        sWaypoints.add(new Waypoint(140,235,0,80));
+        sWaypoints.add(new Waypoint(240,220,0,80,    "shiftLow"));
+        sWaypoints.add(new Waypoint(270,215,0,80,   "raiseElevator"));
+        sWaypoints.add(new Waypoint(310,215,20,80));
+        sWaypoints.add(new Waypoint(310,235,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(18, 51), Rotation2d.fromDegrees(180)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeftAPRV3.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class RightStartToScaleLeftAPRV3 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(18,51,0,40));
+        sWaypoints.add(new Waypoint(55,51,20,40));
+        sWaypoints.add(new Waypoint(55,290,80,100));
+        sWaypoints.add(new Waypoint(313,300,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(18, 51), Rotation2d.fromDegrees(180)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeftSave.java

@@ -0,0 +1,42 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class RightStartToScaleLeftSave implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(18,51,0,110));
+        sWaypoints.add(new Waypoint(159,51,0,110));
+        sWaypoints.add(new Waypoint(220,51,40,110));
+        sWaypoints.add(new Waypoint(218,115,0,60));
+        sWaypoints.add(new Waypoint(218,145,0,100));
+        sWaypoints.add(new Waypoint(218,175,0,100));
+        sWaypoints.add(new Waypoint(218,205,0,100));
+        sWaypoints.add(new Waypoint(218,234,25,50));
+        sWaypoints.add(new Waypoint(250,228,0,50,       "raiseElevator"));
+        sWaypoints.add(new Waypoint(271,226,0,40));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(18, 51), Rotation2d.fromDegrees(180)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleLeftSaveV2.java

@@ -0,0 +1,43 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class RightStartToScaleLeftSaveV2 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(18,51,0,110));
+        sWaypoints.add(new Waypoint(60,51,0,130,    "shiftHi"));
+        sWaypoints.add(new Waypoint(159,51,0,130));
+        sWaypoints.add(new Waypoint(220,51,30,110));
+        sWaypoints.add(new Waypoint(218,115,0,60));
+        sWaypoints.add(new Waypoint(218,145,0,130));
+        sWaypoints.add(new Waypoint(218,175,0,130));
+        sWaypoints.add(new Waypoint(218,205,0,130));
+        sWaypoints.add(new Waypoint(218,234,25,50));
+        sWaypoints.add(new Waypoint(250,234,0,50,     "raiseElevator"));
+        sWaypoints.add(new Waypoint(270,231,0,40));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(18, 51), Rotation2d.fromDegrees(180)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleRight.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class RightStartToScaleRight implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(18,51,0,100));
+        sWaypoints.add(new Waypoint(180,51,30,100,       "raiseElevator"));
+        sWaypoints.add(new Waypoint(240,86,30,100));
+        sWaypoints.add(new Waypoint(273,84,0,100));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(18, 51), Rotation2d.fromDegrees(180)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToScaleRightV2.java

@@ -0,0 +1,37 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class RightStartToScaleRightV2 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(18,51,0,130));
+        sWaypoints.add(new Waypoint(40,51,0,130,        "shiftHi"));
+        sWaypoints.add(new Waypoint(145,52,55,130,                   "raiseElevator"));
+        sWaypoints.add(new Waypoint(257,93,0,130,              "startEject"));
+        sWaypoints.add(new Waypoint(282,100,0,130));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(18, 51), Rotation2d.fromDegrees(180)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToSwitchLeftV3.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class RightStartToSwitchLeftV3 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(78,55,0,100));
+        sWaypoints.add(new Waypoint(78,181,20,100,      "raiseElevator"));
+        sWaypoints.add(new Waypoint(105,190,0,70));
+        sWaypoints.add(new Waypoint(134,194,0,40));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(78, 55), Rotation2d.fromDegrees(90.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightStartToSwitchRightV3.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class RightStartToSwitchRightV3 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(65,55,0,60));
+        sWaypoints.add(new Waypoint(65,94,30,60, "raiseElevator"));
+        sWaypoints.add(new Waypoint(110,94,0,60));
+        sWaypoints.add(new Waypoint(126,94,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(65, 55), Rotation2d.fromDegrees(90.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/RightSwitch2ndCubeV2.java

@@ -0,0 +1,35 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class RightSwitch2ndCubeV2 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(230,89,0,60));
+        sWaypoints.add(new Waypoint(210,105,0,40));
+        sWaypoints.add(new Waypoint(201,120,0,20));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(230, 89), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToOuttaHere.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class ScaleLeftToOuttaHere implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(287,240,0,80));
+        sWaypoints.add(new Waypoint(287,285,20,80));
+        sWaypoints.add(new Waypoint(314,285,0,80));
+        sWaypoints.add(new Waypoint(329,276,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(287, 240), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeft.java

@@ -0,0 +1,35 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class ScaleLeftToSwitchLeft implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(291,247,0,60));
+        sWaypoints.add(new Waypoint(230,238,0,60));
+        sWaypoints.add(new Waypoint(217,236,0,20));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(291, 247), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeft2.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class ScaleLeftToSwitchLeft2 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(291,241,0,60));
+        sWaypoints.add(new Waypoint(248,241,30,60));
+        sWaypoints.add(new Waypoint(230,215,0,40));
+        sWaypoints.add(new Waypoint(218,200,0,20));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(291, 241), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeft2V2.java

@@ -0,0 +1,37 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class ScaleLeftToSwitchLeft2V2 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(291,241,0,60));
+        sWaypoints.add(new Waypoint(248,241,15,60));
+        sWaypoints.add(new Waypoint(228,225,0,40));
+        sWaypoints.add(new Waypoint(220,215,10,20));
+        sWaypoints.add(new Waypoint(218,200,0,20));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(291, 241), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeftNoIntake.java

@@ -0,0 +1,35 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class ScaleLeftToSwitchLeftNoIntake implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(276,247,0,60));
+        sWaypoints.add(new Waypoint(232,237,0,60));
+        sWaypoints.add(new Waypoint(225,236,0,30));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(276, 247), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeftV2.java

@@ -0,0 +1,35 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class ScaleLeftToSwitchLeftV2 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(267,240,0,60));
+        sWaypoints.add(new Waypoint(224,233,0,60));
+        sWaypoints.add(new Waypoint(214,231,0,40));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(267, 240), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleLeftToSwitchLeftV3.java

@@ -0,0 +1,35 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class ScaleLeftToSwitchLeftV3 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(281,247,0,60));
+        sWaypoints.add(new Waypoint(230,238,0,60));
+        sWaypoints.add(new Waypoint(218,236,0,20));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(281, 247), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightSideToSwitchRight.java

@@ -0,0 +1,34 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class ScaleRightSideToSwitchRight implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(330,40,0,80));
+        sWaypoints.add(new Waypoint(254,73,0,80));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(330, 40), Rotation2d.fromDegrees(155)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToOuttaHere.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class ScaleRightToOuttaHere implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(287,85,0,80));
+        sWaypoints.add(new Waypoint(287,40,20,80));
+        sWaypoints.add(new Waypoint(314,40,0,80));
+        sWaypoints.add(new Waypoint(335,50,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(287, 85), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToPyramid.java

@@ -0,0 +1,39 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class ScaleRightToPyramid implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(282,80,0,130));
+        sWaypoints.add(new Waypoint(230,65,30,130,  "shiftHi"));
+        sWaypoints.add(new Waypoint(105,62,0,130,      "shiftLo"));
+        sWaypoints.add(new Waypoint(60,62,40,60));
+        sWaypoints.add(new Waypoint(60,146,20,60,      "intakeOn"));
+        sWaypoints.add(new Waypoint(80,160,0,60));
+        sWaypoints.add(new Waypoint(150,160,0,40));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(282, 80), Rotation2d.fromDegrees(180)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRight.java

@@ -0,0 +1,35 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class ScaleRightToSwitchRight implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(287,85,0,40));
+        sWaypoints.add(new Waypoint(234,91,0,40));
+        sWaypoints.add(new Waypoint(214,94,0,20));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(287, 85), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRight2.java

@@ -0,0 +1,35 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class ScaleRightToSwitchRight2 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(297,85,0,0));
+        sWaypoints.add(new Waypoint(254,84,20,60));
+        sWaypoints.add(new Waypoint(219,111,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(297, 85), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRight23Cube.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class ScaleRightToSwitchRight23Cube implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(291,84,0,60));
+        sWaypoints.add(new Waypoint(248,84,30,60));
+        sWaypoints.add(new Waypoint(230,110,0,40));
+        sWaypoints.add(new Waypoint(218,125,0,20));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(291, 84), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRight3Cube.java

@@ -0,0 +1,35 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class ScaleRightToSwitchRight3Cube implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(291,78,0,60));
+        sWaypoints.add(new Waypoint(230,87,0,60));
+        sWaypoints.add(new Waypoint(218,89,0,20));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(291, 78), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRightNoIntake.java

@@ -0,0 +1,35 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class ScaleRightToSwitchRightNoIntake implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(276,78,0,60));
+        sWaypoints.add(new Waypoint(232,88,0,60));
+        sWaypoints.add(new Waypoint(225,89,0,30));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(276, 78), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRightV2.java

@@ -0,0 +1,35 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class ScaleRightToSwitchRightV2 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(267,85,0,60));
+        sWaypoints.add(new Waypoint(224,92,0,60));
+        sWaypoints.add(new Waypoint(214,94,0,40));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(267, 85), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/ScaleRightToSwitchRightV3.java

@@ -0,0 +1,35 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class ScaleRightToSwitchRightV3 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(281,78,0,60));
+        sWaypoints.add(new Waypoint(230,87,0,60));
+        sWaypoints.add(new Waypoint(218,89,0,20));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(281, 78), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeft2ToScaleLeft.java

@@ -0,0 +1,35 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class SwitchLeft2ToScaleLeft implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(219,213,0,60));
+        sWaypoints.add(new Waypoint(248,235,20,60,   "raiseElevator"));
+        sWaypoints.add(new Waypoint(291,221,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(219, 213), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToCenterStart.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class SwitchLeftToCenterStart implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(116,220,0,40));
+        sWaypoints.add(new Waypoint(100,215,15,40));
+        sWaypoints.add(new Waypoint(74,184,15,40));
+        sWaypoints.add(new Waypoint(54,184,0,40));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(116, 220), Rotation2d.fromDegrees(0.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToCenterStartV2.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class SwitchLeftToCenterStartV2 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(116,220,0,60));
+        sWaypoints.add(new Waypoint(100,215,15,60));
+        sWaypoints.add(new Waypoint(64,172,0,60));
+        sWaypoints.add(new Waypoint(54,164,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(116, 220), Rotation2d.fromDegrees(0.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToNullZone.java

@@ -0,0 +1,37 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class SwitchLeftToNullZone implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(72,246,0,40));
+        sWaypoints.add(new Waypoint(97,250,10,40));
+        sWaypoints.add(new Waypoint(132,280,20,60));
+        sWaypoints.add(new Waypoint(242,270,10,60));
+        sWaypoints.add(new Waypoint(242,250,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(72, 246), Rotation2d.fromDegrees(0.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToScaleLeft.java

@@ -0,0 +1,35 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class SwitchLeftToScaleLeft implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(224,234,0,60));
+        sWaypoints.add(new Waypoint(231,234,0,60,       "raiseElevator"));
+        sWaypoints.add(new Waypoint(291,237,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(224, 234), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToScaleRight.java

@@ -0,0 +1,38 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class SwitchLeftToScaleRight implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(224,234,0,60));
+        sWaypoints.add(new Waypoint(236,234,10,80));
+        sWaypoints.add(new Waypoint(232,154,0,80));
+        sWaypoints.add(new Waypoint(232,95,25,80));
+        sWaypoints.add(new Waypoint(271,98,0,50,     "raiseElevator"));
+        sWaypoints.add(new Waypoint(301,107,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(224, 234), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchLeftToScaleRightV2.java

@@ -0,0 +1,37 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class SwitchLeftToScaleRightV2 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(224,234,0,60));
+        sWaypoints.add(new Waypoint(236,234,10,60));
+        sWaypoints.add(new Waypoint(231,154,0,60,         "raiseElevator"));
+        sWaypoints.add(new Waypoint(231,89,25,50));
+        sWaypoints.add(new Waypoint(277,94,0,40));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(224, 234), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRight2ToScaleRight.java

@@ -0,0 +1,35 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class SwitchRight2ToScaleRight implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(219,111,0,0));
+        sWaypoints.add(new Waypoint(254,84,20,60));
+        sWaypoints.add(new Waypoint(297,85,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(219, 111), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRight2ToScaleRight3Cube.java

@@ -0,0 +1,35 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class SwitchRight2ToScaleRight3Cube implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(219,112,0,60));
+        sWaypoints.add(new Waypoint(248,90,20,60,     "raiseElevator"));
+        sWaypoints.add(new Waypoint(291,104,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(219, 112), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToCenterStart.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class SwitchRightToCenterStart implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(116,104,0,50));
+        sWaypoints.add(new Waypoint(100,109,15,50));
+        sWaypoints.add(new Waypoint(74,145,15,50));
+        sWaypoints.add(new Waypoint(54,145,0,50));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(116, 104), Rotation2d.fromDegrees(0.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToCenterStartV2.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class SwitchRightToCenterStartV2 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(116,104,0,80));
+        sWaypoints.add(new Waypoint(100,109,15,80));
+        sWaypoints.add(new Waypoint(74,140,15,80));
+        sWaypoints.add(new Waypoint(54,140,0,50));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(116, 104), Rotation2d.fromDegrees(0.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToScaleLeft.java

@@ -0,0 +1,38 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class SwitchRightToScaleLeft implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(224,91,0,60));
+        sWaypoints.add(new Waypoint(236,91,10,80));
+        sWaypoints.add(new Waypoint(232,171,0,80));
+        sWaypoints.add(new Waypoint(232,230,25,80));
+        sWaypoints.add(new Waypoint(271,227,0,50,      "raiseElevator"));
+        sWaypoints.add(new Waypoint(301,218,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(224, 91), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToScaleLeftV2.java

@@ -0,0 +1,37 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class SwitchRightToScaleLeftV2 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(224,91,0,60));
+        sWaypoints.add(new Waypoint(236,91,10,60));
+        sWaypoints.add(new Waypoint(231,171,0,60,          "raiseElevator"));
+        sWaypoints.add(new Waypoint(231,236,25,50));
+        sWaypoints.add(new Waypoint(277,231,0,40));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(224, 91), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToScaleRight.java

@@ -0,0 +1,35 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+public class SwitchRightToScaleRight implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(224,95,0,0));
+        sWaypoints.add(new Waypoint(231,95,0,60,   "raiseElevator"));
+        sWaypoints.add(new Waypoint(261,88,0,60));
+        sWaypoints.add(new Waypoint(291,88,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(224, 95), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/auton/SwitchRightToScaleRight3Cube.java

@@ -0,0 +1,35 @@
+package org.usfirst.frc4388.paths.auton;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+
+public class SwitchRightToScaleRight3Cube implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(224,91,0,60));
+        sWaypoints.add(new Waypoint(231,91,0,60,        "raiseElevator"));
+        sWaypoints.add(new Waypoint(291,88,0,60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(224, 91), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/CompBot.java

@@ -0,0 +1,48 @@
+package org.usfirst.frc4388.paths.profiles;
+
+/**
+ * Contains the corrective values for Comp bot
+ */
+public class CompBot implements RobotProfile {
+
+    @Override
+    public double getRedBoilerGearXCorrection() {
+        return 3.0;
+    }
+
+    @Override
+    public double getRedBoilerGearYCorrection() {
+        return 4.0;
+    }
+
+    @Override
+    public double getRedHopperXOffset() {
+        return 0.0;
+    }
+
+    @Override
+    public double getRedHopperYOffset() {
+        return -3.0;
+    }
+
+    @Override
+    public double getBlueBoilerGearXCorrection() {
+        return 0.5;
+    }
+
+    @Override
+    public double getBlueBoilerGearYCorrection() {
+        return 1.0;
+    }
+
+    @Override
+    public double getBlueHopperXOffset() {
+        return -5.5;
+    }
+
+    @Override
+    public double getBlueHopperYOffset() {
+        return 0.0;
+    }
+
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/DalyField.java

@@ -0,0 +1,48 @@
+package org.usfirst.frc4388.paths.profiles;
+
+/**
+ * Contains the measurements for the Daly field at St. Louis champs
+ */
+public class DalyField implements FieldProfile {
+
+    @Override
+    public double getRedCenterToBoiler() {
+        return 125.44;
+    }
+
+    @Override
+    public double getRedWallToAirship() {
+        return 114.5;
+    }
+
+    @Override
+    public double getRedCenterToHopper() {
+        return 162;
+    }
+
+    @Override
+    public double getRedWallToHopper() { // TODO: verify this
+        return 110.5;
+    }
+
+    @Override
+    public double getBlueCenterToBoiler() {
+        return 126.76;
+    }
+
+    @Override
+    public double getBlueWallToAirship() {
+        return 113.75;
+    }
+
+    @Override
+    public double getBlueCenterToHopper() {
+        return 162.25;
+    }
+
+    @Override
+    public double getBlueWallToHopper() {
+        return 109.625;
+    }
+
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/FieldProfile.java

@@ -0,0 +1,26 @@
+package org.usfirst.frc4388.paths.profiles;
+
+/**
+ * Interface that holds all the field measurements required by the PathAdapter
+ * 
+ * @see PathAdapter
+ */
+public interface FieldProfile {
+
+    public double getRedCenterToBoiler();
+
+    public double getRedWallToAirship();
+
+    public double getRedCenterToHopper();
+
+    public double getRedWallToHopper();
+
+    public double getBlueCenterToBoiler();
+
+    public double getBlueWallToAirship();
+
+    public double getBlueCenterToHopper();
+
+    public double getBlueWallToHopper();
+
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/PathAdapter.java

@@ -0,0 +1,261 @@
+package org.usfirst.frc4388.paths.profiles;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.robot.Constants;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+/**
+ * Uses a field and robot profile to calculate Waypoints for the paths used by the GearThenHopperShoot auto modes.
+ * 
+ * @see RobotProfile
+ * @see FieldProfile
+ */
+public class PathAdapter {
+
+    static final RobotProfile kRobotProfile = new CompBot();
+    static final FieldProfile kFieldProfile = new PracticeField();
+
+    // Path Variables
+    static final double kLargeRadius = 45;
+    static final double kModerateRadius = 30;
+    static final double kNominalRadius = 20;
+    static final double kSmallRadius = 10;
+    static final double kSpeed = 80;
+
+    // Don't mess with these
+    static final double kPegOffsetX = 17.77; // center of airship to boiler peg
+    static final double kPegOffsetY = 30.66; // front of airship to boiler
+                                             // pegkRobotProfile.getBlueBoilerGearXCorrection()
+    static final Rotation2d kRedPegHeading = Rotation2d.fromDegrees(240);
+    static final Rotation2d kBluePegHeading = Rotation2d.fromDegrees(125);
+    static final Rotation2d kRedHopperHeading = Rotation2d.fromDegrees(45); // angle to hit the red hopper at
+    static final Rotation2d kBlueHopperHeading = Rotation2d.fromDegrees(315); // angle to hit the blue hopper at
+    static final Rotation2d kStartHeading = Rotation2d.fromDegrees(180); // start angle (backwards)
+    static final double kGearPlacementDist = Constants.kCenterToRearBumperDistance + 10; // distance away from the
+                                                                                         // airship wall to place the
+                                                                                         // gear at
+    static final double kHopperOffsetX = 3.0; // How far from the closest edge of the hopper to aim
+    static final double kHopperSkew = 6.0; // How far into the wall to place the final point (to ensure we keep nudging
+                                           // into the wall)
+    static final double kFrontDist = Constants.kCenterToIntakeDistance;
+    static final double kSideDist = Constants.kCenterToSideBumperDistance;
+    static final double kHopperTurnDistance = 40; // how long the third segment in the hopper path should be
+    static final double kGearTurnDistance = 24; // how long the first segment in the hopper path should be
+    static final double kEndHopperPathX = 84.5; // X position we want the hopper path to end at
+    static final double kFieldHeight = 324; // total height of the field in inches (doesn't really have to be accurate,
+                                            // everything is relative)
+
+    public static Translation2d getRedHopperPosition() {
+        Translation2d contactPoint = new Translation2d(
+                kFieldProfile.getRedWallToHopper() + kHopperOffsetX + kRobotProfile.getRedHopperXOffset(),
+                kFieldHeight / 2 - kFieldProfile.getRedCenterToHopper() - kRobotProfile.getRedHopperYOffset());
+        Translation2d robotOffset = new Translation2d(kFrontDist, kSideDist);
+        robotOffset = robotOffset.direction().rotateBy(kRedHopperHeading).toTranslation().scale(robotOffset.norm());
+        return contactPoint.translateBy(robotOffset);
+    }
+
+    // third point in the hopper path
+    public static Translation2d getRedHopperTurnPosition() {
+        Translation2d hopperPosition = getRedHopperPosition();
+        Translation2d turnOffset = new Translation2d(kRedHopperHeading.cos() * kHopperTurnDistance,
+                kRedHopperHeading.sin() * kHopperTurnDistance);
+        return hopperPosition.translateBy(turnOffset);
+    }
+
+    // second point in the hopper path
+    public static Translation2d getRedGearTurnPosition() {
+        Translation2d gearPosition = getRedGearPosition();
+        Translation2d turnOffset = new Translation2d(kRedPegHeading.cos() * kGearTurnDistance,
+                kRedPegHeading.sin() * kGearTurnDistance);
+        return gearPosition.translateBy(turnOffset);
+    }
+
+    public static Translation2d getRedGearCorrection() {
+        return RigidTransform2d.fromRotation(kRedPegHeading)
+                .transformBy(RigidTransform2d
+                        .fromTranslation((new Translation2d(-kRobotProfile.getRedBoilerGearXCorrection(),
+                                -kRobotProfile.getRedBoilerGearYCorrection()))))
+                .getTranslation();
+    }
+
+    // final position in the gear path, first position in the hopper path
+    public static Translation2d getRedGearPosition() {
+        Translation2d pegPosition = new Translation2d(kFieldProfile.getRedWallToAirship() + kPegOffsetX,
+                kFieldHeight / 2 - kPegOffsetY);
+        Translation2d robotOffset = new Translation2d(kRedPegHeading.cos() * kGearPlacementDist,
+                kRedPegHeading.sin() * kGearPlacementDist);
+        return pegPosition.translateBy(robotOffset);
+    }
+
+    private static Translation2d getRedGearPositionCorrected() {
+        return getRedGearPosition().translateBy(getRedGearCorrection());
+    }
+
+    // first position in the gear path
+    public static RigidTransform2d getRedStartPose() {
+        return new RigidTransform2d(new Translation2d(Constants.kCenterToFrontBumperDistance,
+                kFieldHeight / 2 - kFieldProfile.getRedCenterToBoiler() + Constants.kCenterToSideBumperDistance),
+                kStartHeading);
+    }
+
+    // second position in the gear path
+    private static Translation2d getRedCenterPosition() {
+        RigidTransform2d end = new RigidTransform2d(getRedGearPositionCorrected(), kRedPegHeading);
+        return getRedStartPose().intersection(end);
+    }
+
+    private static Path sRedGearPath = null;
+
+    public static Path getRedGearPath() {
+        if (sRedGearPath == null) {
+            ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+            sWaypoints.add(new Waypoint(getRedStartPose().getTranslation(), 0, kSpeed));
+            sWaypoints.add(new Waypoint(getRedCenterPosition(), kLargeRadius, kSpeed));
+            sWaypoints.add(new Waypoint(getRedGearPositionCorrected(), 0, kSpeed));
+
+            sRedGearPath = PathBuilder.buildPathFromWaypoints(sWaypoints);
+        }
+        return sRedGearPath;
+    }
+
+    private static Path sRedHopperPath = null;
+
+    public static Path getRedHopperPath() {
+        if (sRedHopperPath == null) {
+            ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+            sWaypoints.add(new Waypoint(getRedGearPosition(), 0, kSpeed));
+            sWaypoints.add(new Waypoint(getRedGearTurnPosition(), kSmallRadius, kSpeed));
+            sWaypoints.add(new Waypoint(getRedHopperTurnPosition(), kModerateRadius, kSpeed));
+            sWaypoints.add(new Waypoint(getRedHopperPosition(), kSmallRadius, kSpeed));
+
+            Translation2d redHopperEndPosition = new Translation2d(getRedHopperPosition());
+            redHopperEndPosition.setX(kEndHopperPathX); // move X position to desired place
+            redHopperEndPosition.setY(redHopperEndPosition.y() - kHopperSkew); // TODO make constant
+            sWaypoints.add(new Waypoint(redHopperEndPosition, 0, kSpeed));
+            sRedHopperPath = PathBuilder.buildPathFromWaypoints(sWaypoints);
+        }
+        return sRedHopperPath;
+
+    }
+
+    public static Translation2d getBlueHopperPosition() {
+        Translation2d contactPoint = new Translation2d(
+                kFieldProfile.getBlueWallToHopper() + kHopperOffsetX + kRobotProfile.getBlueHopperXOffset(),
+                kFieldHeight / 2 + kFieldProfile.getBlueCenterToHopper() + kRobotProfile.getBlueHopperYOffset());
+        Translation2d robotOffset = new Translation2d(kFrontDist, -kSideDist);
+        robotOffset = robotOffset.direction().rotateBy(kBlueHopperHeading).toTranslation().scale(robotOffset.norm());
+        return contactPoint.translateBy(robotOffset);
+    }
+
+    public static Translation2d getBlueHopperTurnPosition() {
+        Translation2d hopperPosition = getBlueHopperPosition();
+        Translation2d turnOffset = new Translation2d(kBlueHopperHeading.cos() * kHopperTurnDistance,
+                kBlueHopperHeading.sin() * kHopperTurnDistance);
+        return hopperPosition.translateBy(turnOffset);
+    }
+
+    public static Translation2d getBlueGearTurnPosition() {
+        Translation2d gearPosition = getBlueGearPosition();
+        Translation2d turnOffset = new Translation2d(kBluePegHeading.cos() * kGearTurnDistance,
+                kBluePegHeading.sin() * kGearTurnDistance);
+        return gearPosition.translateBy(turnOffset);
+    }
+
+    public static Translation2d getBlueGearCorrection() {
+        return RigidTransform2d.fromRotation(kBluePegHeading)
+                .transformBy(RigidTransform2d
+                        .fromTranslation((new Translation2d(-kRobotProfile.getBlueBoilerGearXCorrection(),
+                                -kRobotProfile.getBlueBoilerGearYCorrection()))))
+                .getTranslation();
+    }
+
+    private static Translation2d getBlueGearPosition() {
+        Translation2d pegPosition = new Translation2d(kFieldProfile.getBlueWallToAirship() + kPegOffsetX,
+                kFieldHeight / 2 + kPegOffsetY);
+        Translation2d robotOffset = new Translation2d(kBluePegHeading.cos() * kGearPlacementDist,
+                kBluePegHeading.sin() * kGearPlacementDist);
+        return pegPosition.translateBy(robotOffset);
+    }
+
+    private static Translation2d getBlueGearPositionCorrected() {
+        return getBlueGearPosition().translateBy(getBlueGearCorrection());
+    }
+
+    public static RigidTransform2d getBlueStartPose() {
+        return new RigidTransform2d(new Translation2d(Constants.kCenterToFrontBumperDistance,
+                kFieldHeight / 2 + kFieldProfile.getBlueCenterToBoiler() - Constants.kCenterToSideBumperDistance),
+                kStartHeading);
+    }
+
+    private static Translation2d getBlueCenterPosition() {
+        RigidTransform2d end = new RigidTransform2d(getBlueGearPositionCorrected(), kBluePegHeading);
+        return getBlueStartPose().intersection(end);
+    }
+
+    private static Path sBlueGearPath = null;
+
+    public static Path getBlueGearPath() {
+        if (sBlueGearPath == null) {
+            ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+            sWaypoints.add(new Waypoint(getBlueStartPose().getTranslation(), 0, kSpeed));
+            sWaypoints.add(new Waypoint(getBlueCenterPosition(), kLargeRadius, kSpeed));
+            sWaypoints.add(new Waypoint(getBlueGearPositionCorrected(), 0, kSpeed));
+
+            sBlueGearPath = PathBuilder.buildPathFromWaypoints(sWaypoints);
+        }
+        return sBlueGearPath;
+    }
+
+    private static Path sBlueHopperPath = null;
+
+    public static Path getBlueHopperPath() {
+        if (sBlueHopperPath == null) {
+            ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+            sWaypoints.add(new Waypoint(getBlueGearPosition(), 0, 0));
+            sWaypoints.add(new Waypoint(getBlueGearTurnPosition(), kSmallRadius, kSpeed));
+            sWaypoints.add(new Waypoint(getBlueHopperTurnPosition(), kModerateRadius, kSpeed));
+            sWaypoints.add(new Waypoint(getBlueHopperPosition(), kSmallRadius, kSpeed));
+
+            Translation2d blueHopperEndPosition = new Translation2d(getBlueHopperPosition());
+            blueHopperEndPosition.setX(kEndHopperPathX); // move x position to desired place
+            blueHopperEndPosition.setY(blueHopperEndPosition.y() + kHopperSkew);
+            sWaypoints.add(new Waypoint(blueHopperEndPosition, 0, kSpeed));
+
+            sBlueHopperPath = PathBuilder.buildPathFromWaypoints(sWaypoints);
+        }
+        return sBlueHopperPath;
+    }
+
+    public static void calculatePaths() {
+        getBlueHopperPath();
+        getRedHopperPath();
+        getBlueGearPath();
+        getRedGearPath();
+    }
+
+    public static void main(String[] args) {
+        System.out.println("Red:\n" + getRedStartPose().getTranslation());
+        System.out.println("Center: " + getRedCenterPosition());
+        System.out.println("Gear: " + getRedGearPositionCorrected());
+        System.out.println("Gear turn: " + getRedGearTurnPosition());
+        System.out.println("Hopper turn: " + getRedHopperTurnPosition());
+        System.out.println("Hopper: " + getRedHopperPosition());
+        System.out.println("Start to boiler gear path:\n" + getRedGearPath());
+        System.out.println("Boiler gear to hopper path:\n" + getRedHopperPath());
+        System.out.println("\nBlue:\n" + getBlueStartPose().getTranslation());
+        System.out.println("Center: " + getBlueCenterPosition());
+        System.out.println("Gear: " + getBlueGearPositionCorrected());
+        System.out.println("Gear turn: " + getBlueGearTurnPosition());
+        System.out.println("Hopper turn: " + getBlueHopperTurnPosition());
+        System.out.println("Hopper: " + getBlueHopperPosition());
+        System.out.println("Start to boiler gear path:\n" + getBlueGearPath());
+        System.out.println("Boiler gear to hopper path:\n" + getBlueHopperPath());
+    }
+
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/PracticeBot.java

@@ -0,0 +1,48 @@
+package org.usfirst.frc4388.paths.profiles;
+
+/**
+ * Contains the corrective values for Practice bot
+ */
+public class PracticeBot implements RobotProfile {
+
+    @Override
+    public double getRedBoilerGearXCorrection() {
+        return 2.5;
+    }
+
+    @Override
+    public double getRedBoilerGearYCorrection() {
+        return 7.0;
+    }
+
+    @Override
+    public double getRedHopperXOffset() {
+        return 0.0;
+    }
+
+    @Override
+    public double getRedHopperYOffset() {
+        return -6.0;
+    }
+
+    @Override
+    public double getBlueBoilerGearXCorrection() {
+        return 2.5;
+    }
+
+    @Override
+    public double getBlueBoilerGearYCorrection() {
+        return -1.0;
+    }
+
+    @Override
+    public double getBlueHopperXOffset() {
+        return 0.0;
+    }
+
+    @Override
+    public double getBlueHopperYOffset() {
+        return -4.0;
+    }
+
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/PracticeField.java

@@ -0,0 +1,48 @@
+package org.usfirst.frc4388.paths.profiles;
+
+/**
+ * Contains the measurements for the practice field at the 254 lab
+ */
+public class PracticeField implements FieldProfile {
+
+    @Override
+    public double getRedCenterToBoiler() {
+        return 127.5;
+    }
+
+    @Override
+    public double getRedWallToAirship() {
+        return 116.5;
+    }
+
+    @Override
+    public double getRedCenterToHopper() {
+        return 160.66;
+    }
+
+    @Override
+    public double getRedWallToHopper() {
+        return 108.0;
+    }
+
+    @Override
+    public double getBlueCenterToBoiler() {
+        return 125.5;
+    }
+
+    @Override
+    public double getBlueWallToAirship() {
+        return 114.0;
+    }
+
+    @Override
+    public double getBlueCenterToHopper() {
+        return 161.0;
+    }
+
+    @Override
+    public double getBlueWallToHopper() {
+        return 110.0;
+    }
+
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/profiles/RobotProfile.java

@@ -0,0 +1,28 @@
+package org.usfirst.frc4388.paths.profiles;
+
+/**
+ * Interface that holds all the corrective values for how each robot actually drives.
+ * 
+ * @see PathAdapter
+ */
+public interface RobotProfile {
+
+    // red
+    public double getRedBoilerGearXCorrection();
+
+    public double getRedBoilerGearYCorrection();
+
+    public double getRedHopperXOffset();
+
+    public double getRedHopperYOffset();
+
+    // blue
+    public double getBlueBoilerGearXCorrection();
+
+    public double getBlueBoilerGearYCorrection();
+
+    public double getBlueHopperXOffset();
+
+    public double getBlueHopperYOffset();
+
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/test/CenterTest.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc4388.paths.test;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+public class CenterTest implements PathContainer {
+
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(0, 0, 0, 0));
+        sWaypoints.add(new Waypoint(-20, 0, 0, 60));
+        sWaypoints.add(new Waypoint(-40, 0, 20, 60));
+        sWaypoints.add(new Waypoint(-60, 20, 20, 60));
+        sWaypoints.add(new Waypoint(-120, 20, 0, 60));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(0, 0), Rotation2d.fromDegrees(180.0));
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true;
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/test/LeftTurn.java

@@ -0,0 +1,37 @@
+package org.usfirst.frc4388.paths.test;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+public class LeftTurn implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(20,  60, 0,  0));
+        sWaypoints.add(new Waypoint(60,  60, 0, 20));
+        sWaypoints.add(new Waypoint(60, 100, 0, 20));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(20, 60), Rotation2d.fromDegrees(0.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+	// WAYPOINT_DATA: [{"position":{"x":20,"y":60},"speed":0,"radius":0,"comment":""},{"position":{"x":60,"y":60},"speed":15,"radius":0,"comment":""},{"position":{"x":60,"y":100},"speed":15,"radius":0,"comment":""}]
+	// IS_REVERSED: false
+	// FILE_NAME: LeftTurn
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/test/LeftTurnRadius.java

@@ -0,0 +1,37 @@
+package org.usfirst.frc4388.paths.test;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+public class LeftTurnRadius implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(20,  60,  0,  0));
+        sWaypoints.add(new Waypoint(60,  60, 30, 20));
+        sWaypoints.add(new Waypoint(60, 100,  0, 20));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(20, 60), Rotation2d.fromDegrees(0.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+	// WAYPOINT_DATA: [{"position":{"x":20,"y":60},"speed":0,"radius":0,"comment":""},{"position":{"x":60,"y":60},"speed":15,"radius":30,"comment":""},{"position":{"x":60,"y":100},"speed":15,"radius":0,"comment":""}]
+	// IS_REVERSED: false
+	// FILE_NAME: LeftTurnRadius
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/test/LeftTurnRadiusReversed.java

@@ -0,0 +1,37 @@
+package org.usfirst.frc4388.paths.test;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+public class LeftTurnRadiusReversed implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(20,  60,  0,  0));
+        sWaypoints.add(new Waypoint(60,  60, 30, 20));
+        sWaypoints.add(new Waypoint(60, 100,  0, 20));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(20, 60), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+	// WAYPOINT_DATA: [{"position":{"x":20,"y":60},"speed":0,"radius":0,"comment":""},{"position":{"x":60,"y":60},"speed":15,"radius":30,"comment":""},{"position":{"x":60,"y":100},"speed":15,"radius":0,"comment":""}]
+	// IS_REVERSED: true
+	// FILE_NAME: LeftTurnRadiusReversed
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/test/LeftTurnRadiusStart90.java

@@ -0,0 +1,37 @@
+package org.usfirst.frc4388.paths.test;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+public class LeftTurnRadiusStart90 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(60,  100,  0,  0));
+        sWaypoints.add(new Waypoint(60,  60, 30, 20));
+        sWaypoints.add(new Waypoint(100, 60,  0, 20));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(60, 100), Rotation2d.fromDegrees(-90.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+	// WAYPOINT_DATA: [{"position":{"x":20,"y":60},"speed":0,"radius":0,"comment":""},{"position":{"x":60,"y":60},"speed":15,"radius":30,"comment":""},{"position":{"x":60,"y":100},"speed":15,"radius":0,"comment":""}]
+	// IS_REVERSED: false
+	// FILE_NAME: LeftTurnRadius
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/test/LeftTurnReversed.java

@@ -0,0 +1,37 @@
+package org.usfirst.frc4388.paths.test;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+public class LeftTurnReversed implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(20,  60, 0,  0));
+        sWaypoints.add(new Waypoint(60,  60, 0, 20));
+        sWaypoints.add(new Waypoint(60, 100, 0, 20));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(20, 60), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+	// WAYPOINT_DATA: [{"position":{"x":20,"y":60},"speed":0,"radius":0,"comment":""},{"position":{"x":60,"y":60},"speed":15,"radius":0,"comment":""},{"position":{"x":60,"y":100},"speed":15,"radius":0,"comment":""}]
+	// IS_REVERSED: true
+	// FILE_NAME: UntitledPath
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/test/RightTurnRadiusStart90.java

@@ -0,0 +1,34 @@
+package org.usfirst.frc4388.paths.test;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+public class RightTurnRadiusStart90 implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint(60,100,0,0));
+        sWaypoints.add(new Waypoint(60,60,30,20));
+        sWaypoints.add(new Waypoint(20,60,0,20));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(60, 100), Rotation2d.fromDegrees(-90.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return false; 
+    }
+}

2019robot/src/main/java/org/usfirst/frc4388/paths/test/SCurveReversed.java

@@ -0,0 +1,38 @@
+package org.usfirst.frc4388.paths.test;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.paths.PathBuilder;
+import org.usfirst.frc4388.paths.PathBuilder.Waypoint;
+import org.usfirst.frc4388.paths.PathContainer;
+import org.usfirst.frc4388.utility.control.Path;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.math.Rotation2d;
+import org.usfirst.frc4388.utility.math.Translation2d;
+
+public class SCurveReversed implements PathContainer {
+    
+    @Override
+    public Path buildPath() {
+        ArrayList<Waypoint> sWaypoints = new ArrayList<Waypoint>();
+        sWaypoints.add(new Waypoint( 20,  0,  0,  0));
+        sWaypoints.add(new Waypoint( 60,  0, 20, 20));
+        sWaypoints.add(new Waypoint(100, 15, 20, 20));
+        sWaypoints.add(new Waypoint(130, 15,  0, 20));
+
+        return PathBuilder.buildPathFromWaypoints(sWaypoints);
+    }
+    
+    @Override
+    public RigidTransform2d getStartPose() {
+        return new RigidTransform2d(new Translation2d(20, 0), Rotation2d.fromDegrees(180.0)); 
+    }
+
+    @Override
+    public boolean isReversed() {
+        return true; 
+    }
+	// WAYPOINT_DATA: [{"position":{"x":20,"y":60},"speed":0,"radius":0,"comment":""},{"position":{"x":60,"y":60},"speed":20,"radius":20,"comment":""},{"position":{"x":100,"y":75},"speed":20,"radius":20,"comment":""},{"position":{"x":130,"y":75},"speed":20,"radius":0,"comment":""}]
+	// IS_REVERSED: true
+	// FILE_NAME: SCurveReversed
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java

@@ -43,6 +43,136 @@ public class Constants {
     public static int kWristEncoderticksPerRev = 4096;
     public static double kWristDegreesOfTravel = 360;
     public static double kWristEncoderTicksPerDegree = 11.38;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+    /* ROBOT PHYSICAL CONSTANTS */
+
+    // Wheels
+    
+    // Drive constants
+
+    // Geometry
+    public static double kCenterToFrontBumperDistance = 16.33;
+    public static double kCenterToIntakeDistance = 23.11;
+    public static double kCenterToRearBumperDistance = 16.99;
+    public static double kCenterToSideBumperDistance = 17.225;
+
+    /* CONTROL LOOP GAINS */
+
+    // PID gains for drive velocity loop (HIGH GEAR)
+    // Units: setpoint, error, and output are in inches per second.
+    public static double kDriveHighGearVelocityKp = 0.2; //1.0;
+    public static double kDriveHighGearVelocityKi = 0.0;
+    public static double kDriveHighGearVelocityKd = 0.04;//0.04;
+    public static double kDriveHighGearVelocityKf = 0.07;//0.07;
+    public static int kDriveHighGearVelocityIZone = 200;
+    public static double kDriveHighGearVelocityRampRate = 0.05; // 0.02
+    public static double kDriveHighGearNominalOutput = 0.5/12.0;
+    public static double kDriveHighGearMaxSetpoint = 17.0 * 12.0; // 17 fps
+
+    // PID gains for drive velocity loop (LOW GEAR)
+    // Units: setpoint, error, and output are in inches per second.
+    public static double kDriveLowGearVelocityKp = 0.1; //.1
+    public static double kDriveLowGearVelocityKi = 0.0;
+    public static double kDriveLowGearVelocityKd = 0.04; // .04
+    public static double kDriveLowGearVelocityKf = 0.03;  // .07
+    public static int kDriveLowGearVelocityIZone = 200;
+    public static double kDriveLowGearVelocityRampRate = 0.02;
+    public static double kDriveLowGearNominalOutput = 0.1/12.0;
+    public static double kDriveLowGearMaxSetpoint = 10.0 * 12.0; // 8 fps
+
+    // PID gains for drive velocity loop (LOW GEAR)
+    // Units: setpoint, error, and output are in inches per second.
+    public static double kDriveLowGearPositionKp = 1.0;  // 1.0
+    public static double kDriveLowGearPositionKi = 0.002;   //0.002
+    public static double kDriveLowGearPositionKd = 0.04;
+    public static double kDriveLowGearPositionKf = .45; // 0.45
+    public static int kDriveLowGearPositionIZone = 700;
+    public static double kDriveLowGearPositionRampRate = 0.05; // V/s
+    public static double kDriveLowGearMaxVelocity = 8.0 * 12.0 * 60.0 / (Math.PI * kDriveWheelDiameterInches); // 8 fps
+                                                                                                               // in RPM
+    public static double kDriveLowGearMaxAccel = 18.0 * 12.0 * 60.0 / (Math.PI * kDriveWheelDiameterInches); // 18 fps/s
+                                                                                                             // in RPM/s
+
+    public static double kDriveVoltageCompensationRampRate = 0.0;
+
+    // Turn to heading gains
+    public static double kDriveTurnKp = 3.0;
+    public static double kDriveTurnKi = 1.5;
+    public static double kDriveTurnKv = 0.0;
+    public static double kDriveTurnKffv = 1.0;
+    public static double kDriveTurnKffa = 0.0;
+    public static double kDriveTurnMaxVel = 360.0;
+    public static double kDriveTurnMaxAcc = 720.0;
+
+    // Path following constants
+    public static double kMinLookAhead = 12.0; // inches
+    public static double kMinLookAheadSpeed = 9.0; // inches per second
+    public static double kMaxLookAhead = 24.0; // inches
+    public static double kMaxLookAheadSpeed = 120.0; // inches per second
+    public static double kDeltaLookAhead = kMaxLookAhead - kMinLookAhead;
+    public static double kDeltaLookAheadSpeed = kMaxLookAheadSpeed - kMinLookAheadSpeed;
+
+    public static double kInertiaSteeringGain = 0.0; // angular velocity command is multiplied by this gain *
+                                                     // our speed
+                                                     // in inches per sec
+//    public static double kSegmentCompletionTolerance = 0.1; // inches
+//    public static double kPathFollowingMaxAccel = 120.0; // inches per second^2
+//    public static double kPathFollowingMaxVel = 120.0; // inches per second
+//    public static double kPathFollowingProfileKp = 0.05;  //5.0
+//    public static double kPathFollowingProfileKi = 0.03;
+//    public static double kPathFollowingProfileKv = 0.02;
+//    public static double kPathFollowingProfileKffv = 1.0;
+//    public static double kPathFollowingProfileKffa = 0.05;
+//    public static double kPathFollowingGoalPosTolerance = 0.75;
+//    public static double kPathFollowingGoalVelTolerance = 12.0;
+//    public static double kPathStopSteeringDistance = 9.0;
+
+    public static double kSegmentCompletionTolerance = 0.1; // inches
+    public static double kPathFollowingMaxAccel = 90.0; // inches per second^2
+    public static double kPathFollowingMaxVel = 150.0; // inches per second
+    public static double kPathFollowingProfileKp = 5.0;  //5.0
+    public static double kPathFollowingProfileKi = 0.03;  // 0.03
+    public static double kPathFollowingProfileKv = 0.02; //0.02
+    public static double kPathFollowingProfileKffv = 1.2;  //1.2
+    public static double kPathFollowingProfileKffa = 0.05;  //0.05
+    public static double kPathFollowingGoalPosTolerance = 0.75;
+    public static double kPathFollowingGoalVelTolerance = 12.0;
+    public static double kPathStopSteeringDistance = 9.0;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
 	
     // CONTROL LOOP GAINS
 
@@ -75,7 +205,6 @@ public class Constants {
 
     // Path following constants
     public static double kPathFollowingLookahead = 24.0; // inches
-    public static double kPathFollowingMaxVel = 120.0; // inches/sec
-    public static double kPathFollowingMaxAccel = 80.0; // inches/sec^2
+
 
 }

2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java

@@ -51,11 +51,11 @@ public class OI
 	        climbUp.whenReleased(new InitiateClimber(false));
 	        
 	        JoystickButton shiftUp = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON);
-	        shiftUp.whenPressed(new DriveSpeedShift(true));
+	        //shiftUp.whenPressed(new DriveSpeedShift());
 	       // shiftUp.whenPressed(new LEDIndicators(true));
 	        
 	        JoystickButton shiftDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON);
-	        shiftDown.whenPressed(new DriveSpeedShift(false));
+	       // shiftDown.whenPressed(new DriveSpeedShift(false));
 		   // shiftDown.whenPressed(new LEDIndicators(false));
 		   
 		   //Wrist
@@ -63,8 +63,7 @@ public class OI
 		   wristManualMode.whenPressed(new WristSetMode(WristControlMode.JOYSTICK_MANUAL));
 			
 		   //Arm
-		   JoystickButton ArmAimAssist = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_JOYSTICK_BUTTON);
-		   ArmAimAssist.whenPressed(new ArmSetMode(ArmControlMode.PID));
+
 
 
 

2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java

@@ -1,115 +1,151 @@
 
 package org.usfirst.frc4388.robot;
 
-import edu.wpi.first.wpilibj.IterativeRobot;
-import edu.wpi.first.wpilibj.CameraServer;
+import java.util.HashMap;
+//import org.usfirst.frc4388.controller.Robot.OperationMode;
+import org.usfirst.frc4388.robot.commands.ArmAutoZero;//
+import org.usfirst.frc4388.robot.Robot.OperationMode;
+import org.usfirst.frc4388.robot.subsystems.Arm;
+import org.usfirst.frc4388.robot.subsystems.Climber;
+import org.usfirst.frc4388.robot.subsystems.Drive;
+import org.usfirst.frc4388.robot.subsystems.Pnumatics;
+import org.usfirst.frc4388.robot.subsystems.Wrist;
+//import org.usfirst.frc4388.utility.ControlLooper;
+import org.usfirst.frc4388.utility.Looper;
+import org.usfirst.frc4388.utility.control.RobotStateEstimator;
+import org.usfirst.frc4388.utility.math.RigidTransform2d;
+import org.usfirst.frc4388.utility.control.RobotState;
+
 import edu.wpi.first.wpilibj.DriverStation;
 //import edu.wpi.first.wpilibj.PowerDistributionPanel;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Scheduler;
-import edu.wpi.first.wpilibj.command.Subsystem;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;;
 
-//import org.usfirst.frc4388.controller.Robot.OperationMode;
-import org.usfirst.frc4388.robot.commands.*;
-
-import org.usfirst.frc4388.robot.OI;
-import org.usfirst.frc4388.robot.subsystems.*;
-import org.usfirst.frc4388.utility.ControlLooper;
-import org.usfirst.frc4388.robot.subsystems.Drive;
-import org.usfirst.frc4388.robot.subsystems.Arm;
-
-import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;;
-
-public class Robot extends IterativeRobot 
+public class Robot extends TimedRobot
 {
 
 	public static OI oi;
+
+	public static final Drive drive = Drive.getInstance();
+	public static final Arm arm = Arm.getInstance();
 	
-	public static final Drive drive = new Drive();
-    public static final Arm arm = new Arm();
-    public static final Climber climber = new Climber();
+
+	public static final Climber climber = new Climber();
 	public static final Pnumatics pnumatics = new Pnumatics();
 	public static final Wrist wrist = new Wrist();
 	public static final long periodMS = 10;
-	public static final ControlLooper controlLoop = new ControlLooper("Main control loop", periodMS);
-
-	public static enum OperationMode { TEST, COMPETITION };
-	public static OperationMode operationMode = OperationMode.TEST;
+	// public static final ControlLooper controlLoop = new ControlLooper("Main
+	// control loop", periodMS);
 
+	public static final Looper controlLoop = new Looper();
 	private SendableChooser<OperationMode> operationModeChooser;
+	
+    private Command autonomousCommand;
 
+	public static enum OperationMode { TEST, PRACTICE, COMPETITION };
+	public static OperationMode operationMode = OperationMode.COMPETITION;
+
+
+	private RobotState robotState =/* org.usfirst.frc4388.utility.control.*/RobotState.getInstance();
+
+	public void zeroAllSensors() {
+		//drive.zeroSensors();
+		robotState.reset(Timer.getFPGATimestamp(), new RigidTransform2d());
+		//drive.zeroSensors();
+		}
+
+
+
+
+
+	@Override
     public void robotInit() 
     {
-    	//Printing game data to riolog
-    	String gameData = DriverStation.getInstance().getGameSpecificMessage();
-    	System.out.println(gameData);
-    	CameraServer.getInstance().startAutomaticCapture();
-    	CameraServer.getInstance().putVideo("res", 300, 220);
     	
-      try {
+   		setPeriod(Constants.kLooperDt * 2);
+		System.out.println("Main loop period = " + getPeriod());
 		oi = OI.getInstance();
-		
-    	controlLoop.addLoopable(drive);
-    	//controlLoop.addLoopable(arm);
-			
-
-        operationModeChooser = new SendableChooser<OperationMode>();
-	    operationModeChooser.addDefault("Test", OperationMode.TEST);
-	    operationModeChooser.addObject("Competition", OperationMode.COMPETITION);
+		controlLoop.register(drive);
+		controlLoop.register(arm);
+		controlLoop.register(RobotStateEstimator.getInstance());
+	
+    	operationModeChooser = new SendableChooser<OperationMode>();
+	    operationModeChooser.addDefault("Competition", OperationMode.COMPETITION);
+	    operationModeChooser.addObject("Test", OperationMode.TEST);
 		SmartDashboard.putData("Operation Mode", operationModeChooser);
-
-	
-
-		//ledLights.setAllLightsOn(false);
-	  } 
-	  catch (Exception e) 
-	  {
-    		System.err.println("An error occurred in robotInit()");
-      }
-    }
-	
-    public void disabledInit(){
+		LiveWindow.setEnabled(false);
+		zeroAllSensors();
 
     }
 	
+	private void setPeriod(double d) {
+	}
+
+	public void robotPeriodic() {
+		updateStatus();
+	}
+ 
+	@Override
+	public void disabledInit() {
+		drive.setLimeLED(false);
+    }
+	@Override
 	public void disabledPeriodic() {
 		Scheduler.getInstance().run();
 		updateStatus();
 	}
-
-    public void autonomousInit() {    	
+	@Override
+    public void autonomousInit() {  
+		controlLoop.start();
+    	drive.setIsRed(getAlliance().equals(Alliance.Red));
+    	arm.resetZeroPosition(Arm.ZERO_POSITION_INCHES);
     	updateChoosers();
-    	controlLoop.start();
     	drive.endGyroCalibration();
     	drive.resetEncoders();
     	drive.resetGyro();
-    	drive.setIsRed(getAlliance().equals(Alliance.Red));
+		drive.setIsRed(getAlliance().equals(Alliance.Red));
+		if (autonomousCommand != null) {
+			autonomousCommand.start();
+		}	
         
 	}
  
 
-    
+    @Override
     public void autonomousPeriodic() {
         Scheduler.getInstance().run();
 		updateStatus();
     }
-
+	@Override
     public void teleopInit() {
-        drive.setToBrakeOnNeutral(false);	
-    	updateChoosers();
+		if (autonomousCommand != null) {
+			autonomousCommand.cancel();
+		}
+
+		operationMode = operationModeChooser.getSelected();
+		
         controlLoop.start();
-    	drive.resetEncoders();
-		drive.endGyroCalibration();
+    	//drive.setShiftState(DriveSpeedShiftState.LO);
+    	drive.endGyroCalibration();
+        zeroAllSensors();
+
+    	if (operationMode != OperationMode.COMPETITION) {
+    		Scheduler.getInstance().add(new ArmAutoZero(false));
+    	}
+    	else {
+            arm.setPositionPID(arm.getPositionInches());
+    	}
+	}
+
 		//Arm.setArmControlMode(Arm.ArmControlMode.JOYSTICK_MANUAL);
 
-        updateStatus();
-    }
-
-
+	@Override
     public void teleopPeriodic() 
     {
         Scheduler.getInstance().run();
@@ -129,11 +165,16 @@ public class Robot extends IterativeRobot
 	{
     	return m_ds.getAlliance();
     }
-    
+    public double getMatchTime() {
+    	return m_ds.getMatchTime();
+	}
+	
 	public void updateStatus() 
 	{
     	drive.updateStatus(operationMode);
-    	arm.updateStatus(operationMode);
+		arm.updateStatus(operationMode);
+		robotState.updateStatus(operationMode);
+
    }
 
 }

2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java

@@ -7,6 +7,8 @@ public class RobotMap {
 	public static final int DRIVER_JOYSTICK_1_USB_ID = 0;
 	public static final int OPERATOR_JOYSTICK_1_USB_ID = 1;
 
+	public static final int pilot = 0;
+	public static final int copilot = 1;
 
 	// MOTORS 
 	
@@ -24,8 +26,8 @@ public class RobotMap {
 	//Arm Motors
 	public static final int ARM_MOTOR1_ID = 8;
 	public static final int ARM_MOTOR2_ID = 9;
-	public static final int CLIMBER_CAN_ID = 10;
-	public static final int FLIP_OUT_CLIMBER = 11;
+	public static final int CLIMBER_LEGS_CAN_ID = 10;
+	public static final int FLIPPY_BOIS_CAN_ID = 11;
 	public static final int CLIMBER_LEFT = 12;
 	public static final int CLIMBER_RIGHT = 13;
 

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmAutoZero.java

@@ -0,0 +1,64 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.robot.subsystems.Arm;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class ArmAutoZero extends Command
+{
+	private double MIN_ELEVATOR_POSITION_CHANGE = 0.05;
+	private double lastElevatorPosition;
+	private int encoderCount;
+	
+	public ArmAutoZero(boolean interrutible) {
+	
+		requires(Robot.arm);
+		setInterruptible(interrutible);
+	}
+
+	@Override
+	protected void initialize() {
+		lastElevatorPosition = Arm.MAX_POSITION_INCHES;
+		Robot.arm.setSpeed(Arm.AUTO_ZERO_SPEED);
+		encoderCount = 0;
+//		System.out.println("Auto zero initialize");
+	}
+
+	@Override
+	protected void execute() {
+		
+	}
+
+	@Override
+	protected boolean isFinished() {
+		Robot.arm.setSpeed(Arm.AUTO_ZERO_SPEED);
+		double currentElevatorPosition = Robot.arm.getPositionInches();
+		double elevatorPositionChange = lastElevatorPosition - currentElevatorPosition;
+		lastElevatorPosition = currentElevatorPosition;
+		boolean test = encoderCount > 2 && Math.abs(elevatorPositionChange) < MIN_ELEVATOR_POSITION_CHANGE && Robot.arm.getAverageMotorCurrent() > Arm.AUTO_ZERO_MOTOR_CURRENT;
+		System.out.println("encoderCount = " + encoderCount + ", test = " + test + ", elevator change = " + elevatorPositionChange + ", current = " + Robot.arm.getAverageMotorCurrent());
+		
+		if (Math.abs(elevatorPositionChange) < MIN_ELEVATOR_POSITION_CHANGE) {
+			encoderCount++;
+		}
+		else {
+			encoderCount = 0;
+		}
+		
+		return test;
+	}
+
+	@Override
+	protected void end() {
+		Robot.arm.setSpeed(0);
+		Robot.arm.resetZeroPosition(Arm.ZERO_POSITION_INCHES);
+		Robot.arm.setPositionPID(Arm.MIN_POSITION_INCHES);
+//		System.out.println("Elevator Zeroed");
+	}
+
+	@Override
+	protected void interrupted() {
+			
+	}
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java

@@ -1,13 +1,14 @@
+
+
 package org.usfirst.frc4388.robot.commands;
 
 import org.usfirst.frc4388.robot.Robot;
 import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode;
-import java.lang.Math;
 
 import edu.wpi.first.wpilibj.command.Command;
 
 /**
- * Description 
+ *
  */
 public class ArmSetMode extends Command {
 
@@ -20,11 +21,14 @@ public class ArmSetMode extends Command {
 
     // Called just before this Command runs the first time
     protected void initialize() {
-    	if (controlMode == ArmControlMode.PID) {
-    		Robot.wrist.setPositionPID(Robot.arm.getYPositionInches());
+    	if (controlMode == ArmControlMode.JOYSTICK_PID) {
+    		Robot.arm.setPositionPID(Robot.arm.getPositionInches());
     	}
     	else if (controlMode == ArmControlMode.JOYSTICK_MANUAL) {
-    		Robot.wrist.setSpeedJoystick(0);
+    		Robot.arm.setSpeedJoystick(0);
+    	}
+    	else {
+    		Robot.arm.setSpeed(0.0);
     	}
     }
 
@@ -45,4 +49,4 @@ public class ArmSetMode extends Command {
     // subsystems is scheduled to run
     protected void interrupted() {
     }
-}
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionPID.java

@@ -28,7 +28,7 @@ public class ArmSetPositionPID extends Command {
 
     // Make this return true when this Command no longer needs to run execute()
     protected boolean isFinished() {
-        return Math.abs(Robot.arm.getYPositionInches() - this.targetPositionInches) < Arm.PID_ERROR_INCHES;
+        return Math.abs(Robot.arm.getPositionInches() - this.targetPositionInches) < Arm.PID_ERROR_INCHES;
     }
 
     // Called once after isFinished returns true

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java

@@ -2,26 +2,23 @@ package org.usfirst.frc4388.robot.commands;
 
 import org.usfirst.frc4388.robot.Robot;
 
-import org.usfirst.frc4388.robot.subsystems.*;
-
 import edu.wpi.first.wpilibj.command.Command;
 
 /**
  *
  */
 public class ArmSetSpeed extends Command {
-
-	private double RaiseSpeed;
 	
-	// Constructor with speed
-    public ArmSetSpeed(double RaiseSpeed) {
-    	this.RaiseSpeed = RaiseSpeed;
-       // requires(Robot.elevatorAuton);
+	private double speed;
+
+    public ArmSetSpeed(double speed) {
+    	this.speed = speed;
+        requires(Robot.arm);
     }
 
     // Called just before this Command runs the first time
     protected void initialize() {
-    	//Robot.elevatorAuton.setRaiseSpeed(RaiseSpeed);
+    	Robot.arm.setSpeed(speed);
     }
 
     // Called repeatedly when this Command is scheduled to run

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnMP.java

@@ -1,42 +0,0 @@
-package org.usfirst.frc4388.robot.commands;
-
-import org.usfirst.frc4388.robot.Robot;
-import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
-import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-public class DriveAbsoluteTurnMP extends Command
-{
-	private double absoluteTurnAngleDeg, maxTurnRateDegPerSec;
-	private MPSoftwareTurnType turnType;
-
-	public DriveAbsoluteTurnMP(double absoluteTurnAngleDeg, double maxTurnRateDegPerSec, MPSoftwareTurnType turnType) {
-		requires(Robot.drive);
-		this.absoluteTurnAngleDeg = absoluteTurnAngleDeg;
-		this.maxTurnRateDegPerSec = maxTurnRateDegPerSec;
-		this.turnType = turnType;
-	}
-
-	protected void initialize() {
-//		if (Robot.drive.isRed() == false) {
-//			absoluteTurnAngleDeg = absoluteTurnAngleDeg * -1;
-//		}
-		Robot.drive.setAbsoluteTurnMP(absoluteTurnAngleDeg, maxTurnRateDegPerSec, turnType);
-	}
-
-	protected void execute() {
-	}
-
-	protected boolean isFinished() {
-		return Robot.drive.isFinished(); 
-	}
-
-	protected void end() {
-		Robot.drive.setControlMode(DriveControlMode.JOYSTICK);
-	}
-
-	protected void interrupted() {
-		end();
-	}
-}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveGyroReset.java

@@ -1,38 +0,0 @@
-package org.usfirst.frc4388.robot.commands;
-
-import org.usfirst.frc4388.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-public class DriveGyroReset extends Command
-{
-	public DriveGyroReset() {
-		requires(Robot.drive);
-	}
-
-	@Override
-	protected void initialize() {
-		Robot.drive.resetGyro();
-		Robot.drive.resetEncoders();
-	}
-
-	@Override
-	protected void execute() {
-		
-	}
-
-	@Override
-	protected boolean isFinished() {
-		return true;
-	}
-
-	@Override
-	protected void end() {
-		
-	}
-
-	@Override
-	protected void interrupted() {
-			
-	}
-}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveRelativeTurnPID.java

@@ -1,38 +0,0 @@
-package org.usfirst.frc4388.robot.commands;
-
-import org.usfirst.frc4388.robot.Robot;
-import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
-import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-public class DriveRelativeTurnPID extends Command
-{
-	private double relativeTurnAngleDeg;
-	private MPSoftwareTurnType turnType;
-
-	public DriveRelativeTurnPID(double relativeTurnAngleDeg, MPSoftwareTurnType turnType) {
-		requires(Robot.drive);
-		this.relativeTurnAngleDeg = relativeTurnAngleDeg;
-		this.turnType = turnType;
-	}
-
-	protected void initialize() {
-		Robot.drive.setRelativeTurnPID(relativeTurnAngleDeg, 0.3, 0.1, turnType);
-	}
-
-	protected void execute() {
-	}
-
-	protected boolean isFinished() {
-		return Robot.drive.isFinished(); 
-	}
-
-	protected void end() {
-		Robot.drive.setControlMode(DriveControlMode.JOYSTICK);
-	}
-
-	protected void interrupted() {
-		end();
-	}
-}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveSpeedShift.java

@@ -1,40 +0,0 @@
-package org.usfirst.frc4388.robot.commands;
-
-import org.usfirst.frc4388.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-public class DriveSpeedShift extends Command
-{
-	public boolean speed;
-	
-	public DriveSpeedShift(boolean speed) {
-		this.speed=speed;
-		requires(Robot.pnumatics);
-	}
-
-	@Override
-	protected void initialize() {
-		Robot.pnumatics.setShiftState(speed);
-	}
-
-	@Override
-	protected void execute() {
-		
-	}
-
-	@Override
-	protected boolean isFinished() {
-		return true;
-	}
-
-	@Override
-	protected void end() {
-		
-	}
-
-	@Override
-	protected void interrupted() {
-			
-	}
-}