Implement working PathPlanner I/O

src/main/java/frc4388/utility/PathPlannerUtil.java

@@ -11,18 +11,35 @@ public class PathPlannerUtil {
     public Optional<Waypoint>[] waypoints;
 
     public static final class Waypoint {
+      public Waypoint() {}
+      public Waypoint(Double anchorPointX, Double anchorPointY, Double prevControlX, Double prevControlY, Double nextControlX, Double nextControlY, Double holonomicAngle, Boolean isReversal, Double velOverride, Boolean isLocked) {
+        this.anchorPoint = anchorPointX == null && anchorPointY == null ? Optional.empty() : Optional.of(Point.of(anchorPointX, anchorPointY));
+        this.prevControl = prevControlX == null && prevControlY == null ? Optional.empty() : Optional.of(Point.of(prevControlX, prevControlY));
+        this.nextControl = nextControlX == null && nextControlY == null ? Optional.empty() : Optional.of(Point.of(nextControlX, nextControlY));
+        this.holonomicAngle = Optional.ofNullable(holonomicAngle);
+        this.isReversal = Optional.ofNullable(isReversal);
+        this.velOverride = Optional.ofNullable(velOverride);
+        this.isLocked = Optional.ofNullable(isLocked);
+      }
       public Optional<Point> anchorPoint;
       public Optional<Point> prevControl;
       public Optional<Point> nextControl;
 
       public static final class Point {
+        public Point() {}
+        public static Point of(Double x, Double y) {
+          Point point = new Point();
+          point.x = Optional.ofNullable(x);
+          point.y = Optional.ofNullable(y);
+          return point;
+        }
         public Optional<Double> x;
         public Optional<Double> y;
       }
 
       public Optional<Double> holonomicAngle;
       public Optional<Boolean> isReversal;
-      public Optional<Boolean> velOverride;
+      public Optional<Double> velOverride;
       public Optional<Boolean> isLocked;
     }