diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index 50764c7..1fae02b 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -164,18 +164,25 @@
 )
 
 py_binary(
-    name = "path_edit",
+    name = "spline_graph",
     srcs = [
-        "basic_window.py",
         "color.py",
+        "spline_drawing.py",
+        "spline_writer.py",
         "path_edit.py",
+        "points.py",
+        "graph.py",
+        "spline_graph.py",
     ],
+    legacy_create_init = False,
     restricted_to = ["//tools:k8"],
     visibility = ["//visibility:public"],
     deps = [
         ":libspline",
         ":python_init",
         "@python_gtk",
+        "@matplotlib_repo//:matplotlib2.7",
+        ":basic_window",
     ],
 )
 
diff --git a/frc971/control_loops/python/basic_window.py b/frc971/control_loops/python/basic_window.py
old mode 100644
new mode 100755
index 78324a3..827fe64
--- a/frc971/control_loops/python/basic_window.py
+++ b/frc971/control_loops/python/basic_window.py
@@ -1,3 +1,4 @@
+#!/usr/bin/python3
 import gi
 gi.require_version('Gtk', '3.0')
 from gi.repository import Gtk
@@ -5,10 +6,10 @@
 from gi.repository import Gdk
 from gi.repository import GdkX11
 import cairo
+from constants import *
 
 identity = cairo.Matrix()
 
-
 # Override the matrix of a cairo context.
 class OverrideMatrix(object):
     def __init__(self, cr, matrix):
@@ -34,7 +35,6 @@
 def quit_main_loop(*args):
     mainloop.quit()
 
-
 def RunApp():
     try:
         mainloop.run()
@@ -49,11 +49,10 @@
     # Draw in response to an expose-event
     def __init__(self):
         super(BaseWindow, self).__init__()
-        #self.window.connect("destroy", quit_main_loop)
 
-        self.set_size_request(640, 600)
+        self.set_size_request(2*SCREEN_SIZE, SCREEN_SIZE)
         self.center = (0, 0)
-        self.shape = (640, 400)
+        self.shape = (2*SCREEN_SIZE, SCREEN_SIZE)
         self.needs_redraw = False
 
     def get_current_scale(self):
@@ -81,3 +80,4 @@
     # Handle the expose-event by drawing
     def handle_draw(self, cr):
         pass
+
diff --git a/frc971/control_loops/python/color.py b/frc971/control_loops/python/color.py
index ec53e82..5634042 100644
--- a/frc971/control_loops/python/color.py
+++ b/frc971/control_loops/python/color.py
@@ -1,9 +1,10 @@
 class Color:
     def __init__(self, r, g, b, a=1.0):
-      self.r = r
-      self.g = g
-      self.b = b
-      self.a = a
+        self.r = r
+        self.g = g
+        self.b = b
+        self.a = a
+
 
 palette = {
     "RED": Color(1, 0, 0),
@@ -16,5 +17,6 @@
     "WHITE": Color(1, 1, 1),
     "GREY": Color(0.5, 0.5, 0.5),
     "LIGHT_GREY": Color(0.75, 0.75, 0.75),
-    "DARK_GREY": Color(0.25, 0.25, 0.25)
+    "DARK_GREY": Color(0.25, 0.25, 0.25),
+    "ORANGE": Color(1, 0.65, 0)
 }
diff --git a/frc971/control_loops/python/constants.py b/frc971/control_loops/python/constants.py
new file mode 100644
index 0000000..7b45215
--- /dev/null
+++ b/frc971/control_loops/python/constants.py
@@ -0,0 +1,28 @@
+import argparse
+
+arg_parser = argparse.ArgumentParser(description='spline_editor')
+arg_parser.add_argument(
+    'size',
+    metavar='N',
+    default=800,
+    type=int,
+    nargs='?',
+    help="size of the screen")
+args = arg_parser.parse_args()
+SCREEN_SIZE = args.size
+
+WIDTH_OF_FIELD_IN_METERS = 8.258302
+
+WIDTH_OF_ROBOT = 0.65
+LENGTH_OF_ROBOT = 0.8
+
+ROBOT_SIDE_TO_BALL_CENTER = 0.15 #Placeholder value
+BALL_RADIUS = 0.165
+ROBOT_SIDE_TO_HATCH_PANEL = 0.1 #Placeholder value
+HATCH_PANEL_WIDTH = 0.4826
+
+def pxToM(p):
+    return p * WIDTH_OF_FIELD_IN_METERS / SCREEN_SIZE
+
+def mToPx(m):
+    return (m*SCREEN_SIZE/WIDTH_OF_FIELD_IN_METERS)
diff --git a/frc971/control_loops/python/drawing_constants.py b/frc971/control_loops/python/drawing_constants.py
new file mode 100644
index 0000000..89979f0
--- /dev/null
+++ b/frc971/control_loops/python/drawing_constants.py
@@ -0,0 +1,314 @@
+import cairo
+from color import Color, palette
+from constants import *
+import numpy as np
+
+
+def set_color(cr, color, a=1):
+    if color.a == 1.0:
+        cr.set_source_rgba(color.r, color.g, color.b, a)
+    else:
+        cr.set_source_rgba(color.r, color.g, color.b, color.a)
+
+
+def draw_px_cross(cr, x, y, length_px, color=palette["RED"]):
+    """Draws a cross with fixed dimensions in pixel space."""
+    set_color(cr, color)
+    cr.move_to(x, y - length_px)
+    cr.line_to(x, y + length_px)
+    cr.stroke()
+
+    cr.move_to(x - length_px, y)
+    cr.line_to(x + length_px, y)
+    cr.stroke()
+    set_color(cr, palette["WHITE"])
+
+
+def draw_px_x(cr, x, y, length_px1, color=palette["BLACK"]):
+    """Draws a x with fixed dimensions in pixel space."""
+    length_px = length_px1 / np.sqrt(2)
+    set_color(cr, color)
+    cr.move_to(x - length_px, y - length_px)
+    cr.line_to(x + length_px, y + length_px)
+    cr.stroke()
+
+    cr.move_to(x - length_px, y + length_px)
+    cr.line_to(x + length_px, y - length_px)
+    cr.stroke()
+    set_color(cr, palette["WHITE"])
+
+
+def draw_control_points(cr, points, width=10, radius=4, color=palette["BLUE"]):
+    for i in range(0, len(points)):
+        draw_px_x(cr, points[i][0], points[i][1], width, color)
+        set_color(cr, color)
+        cr.arc(points[i][0], points[i][1], radius, 0, 2.0 * np.pi)
+        cr.fill()
+        set_color(cr, palette["WHITE"])
+
+
+def display_text(cr, text, widtha, heighta, widthb, heightb):
+    cr.scale(widtha, -heighta)
+    cr.show_text(text)
+    cr.scale(widthb, -heightb)
+
+
+def draw_HAB(cr):
+    # BASE Constants
+    X_BASE = 0
+    Y_BASE = 0
+    R = 0.381 - .1
+    BACKWALL_X = X_BASE
+    LOADING_Y = mToPx(4.129151) - mToPx(0.696976)
+    # HAB Levels 2 and 3 called in variables backhab
+    # draw loading stations
+    cr.move_to(0, LOADING_Y)
+    cr.line_to(mToPx(0.6), LOADING_Y)
+    cr.move_to(mToPx(R), LOADING_Y)
+    cr.arc(mToPx(R), LOADING_Y, 5, 0, np.pi * 2.0)
+    cr.move_to(0, -1.0 * LOADING_Y)
+    cr.line_to(mToPx(0.6), -1.0 * LOADING_Y)
+    cr.move_to(mToPx(R), -1.0 * LOADING_Y)
+    cr.arc(mToPx(R), -1.0 * LOADING_Y, 5, 0, np.pi * 2.0)
+
+    # HAB Levels 2 and 3 called in variables backhab
+    WIDTH_BACKHAB = mToPx(1.2192)
+
+    Y_TOP_BACKHAB_BOX = Y_BASE + mToPx(0.6096)
+    BACKHAB_LV2_LENGTH = mToPx(1.016)
+
+    BACKHAB_LV3_LENGTH = mToPx(1.2192)
+    Y_LV3_BOX = Y_TOP_BACKHAB_BOX - BACKHAB_LV3_LENGTH
+
+    Y_BOTTOM_BACKHAB_BOX = Y_LV3_BOX - BACKHAB_LV2_LENGTH
+
+    # HAB LEVEL 1
+    X_LV1_BOX = BACKWALL_X + WIDTH_BACKHAB
+
+    WIDTH_LV1_BOX = mToPx(0.90805)
+    LENGTH_LV1_BOX = mToPx(1.6256)
+
+    Y_BOTTOM_LV1_BOX = Y_BASE - LENGTH_LV1_BOX
+
+    # Ramp off Level 1
+    X_RAMP = X_LV1_BOX
+
+    Y_TOP_RAMP = Y_BASE + LENGTH_LV1_BOX
+    WIDTH_TOP_RAMP = mToPx(1.20015)
+    LENGTH_TOP_RAMP = Y_BASE + mToPx(0.28306)
+
+    X_MIDDLE_RAMP = X_RAMP + WIDTH_LV1_BOX
+    Y_MIDDLE_RAMP = Y_BOTTOM_LV1_BOX
+    LENGTH_MIDDLE_RAMP = 2 * LENGTH_LV1_BOX
+    WIDTH_MIDDLE_RAMP = WIDTH_TOP_RAMP - WIDTH_LV1_BOX
+
+    Y_BOTTOM_RAMP = Y_BASE - LENGTH_LV1_BOX - LENGTH_TOP_RAMP
+
+    # Side Bars to Hold in balls
+    X_BARS = BACKWALL_X
+    WIDTH_BARS = WIDTH_BACKHAB
+    LENGTH_BARS = mToPx(0.574675)
+
+    Y_TOP_BAR = Y_TOP_BACKHAB_BOX + BACKHAB_LV2_LENGTH
+
+    Y_BOTTOM_BAR = Y_BOTTOM_BACKHAB_BOX - LENGTH_BARS
+
+    set_color(cr, palette["BLACK"])
+    cr.rectangle(BACKWALL_X, Y_TOP_BACKHAB_BOX, WIDTH_BACKHAB,
+                 BACKHAB_LV2_LENGTH)
+    cr.rectangle(BACKWALL_X, Y_LV3_BOX, WIDTH_BACKHAB, BACKHAB_LV3_LENGTH)
+    cr.rectangle(BACKWALL_X, Y_BOTTOM_BACKHAB_BOX, WIDTH_BACKHAB,
+                 BACKHAB_LV2_LENGTH)
+    cr.rectangle(X_LV1_BOX, Y_BASE, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
+    cr.rectangle(X_LV1_BOX, Y_BOTTOM_LV1_BOX, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
+    cr.rectangle(X_RAMP, Y_TOP_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
+    cr.rectangle(X_MIDDLE_RAMP, Y_MIDDLE_RAMP, WIDTH_MIDDLE_RAMP,
+                 LENGTH_MIDDLE_RAMP)
+    cr.rectangle(X_RAMP, Y_BOTTOM_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
+    cr.rectangle(X_BARS, Y_TOP_BAR, WIDTH_BARS, LENGTH_BARS)
+    cr.rectangle(X_BARS, Y_BOTTOM_BAR, WIDTH_BARS, LENGTH_BARS)
+    cr.stroke()
+
+    cr.set_line_join(cairo.LINE_JOIN_ROUND)
+
+    cr.stroke()
+
+    #draw 0, 0
+    set_color(cr, palette["BLACK"])
+    cr.move_to(0, 0)
+    cr.line_to(0, 0 + mToPx(8.2296 / 2.0))
+    cr.move_to(0, 0)
+    cr.line_to(0, 0 + mToPx(-8.2296 / 2.0))
+    cr.move_to(0, 0)
+    cr.line_to(0 + mToPx(8.2296), 0)
+
+    cr.stroke()
+
+
+def draw_rockets(cr):
+    # BASE Constants
+    X_BASE = mToPx(2.41568)
+    Y_BASE = 0
+    # Robot longitudinal radius
+    R = 0.381
+    near_side_rocket_center = [
+        X_BASE + mToPx((2.89973 + 3.15642) / 2.0), Y_BASE + mToPx(
+            (3.86305 + 3.39548) / 2.0)
+    ]
+    middle_rocket_center = [
+        X_BASE + mToPx((3.15642 + 3.6347) / 2.0), Y_BASE + mToPx(
+            (3.39548 + 3.392380) / 2.0)
+    ]
+    far_side_rocket_center = [
+        X_BASE + mToPx((3.63473 + 3.89984) / 2.0), Y_BASE + mToPx(
+            (3.39238 + 3.86305) / 2.0)
+    ]
+
+    cr.move_to(near_side_rocket_center[0], near_side_rocket_center[1])
+    cr.line_to(near_side_rocket_center[0] - 0.8 * mToPx(0.866),
+               near_side_rocket_center[1] - 0.8 * mToPx(0.5))
+    cr.move_to(near_side_rocket_center[0] - R * mToPx(0.866),
+               near_side_rocket_center[1] - R * mToPx(0.5))
+    cr.arc(near_side_rocket_center[0] - R * mToPx(0.866),
+           near_side_rocket_center[1] - R * mToPx(0.5), 5, 0, np.pi * 2.0)
+
+    cr.move_to(middle_rocket_center[0], middle_rocket_center[1])
+    cr.line_to(middle_rocket_center[0], middle_rocket_center[1] - mToPx(0.8))
+    cr.move_to(middle_rocket_center[0], middle_rocket_center[1] - mToPx(R))
+    cr.arc(middle_rocket_center[0], middle_rocket_center[1] - mToPx(R), 5, 0,
+           np.pi * 2.0)
+
+    cr.move_to(far_side_rocket_center[0], far_side_rocket_center[1])
+    cr.line_to(far_side_rocket_center[0] + 0.8 * mToPx(0.866),
+               far_side_rocket_center[1] - 0.8 * mToPx(0.5))
+    cr.move_to(far_side_rocket_center[0] + R * mToPx(0.866),
+               far_side_rocket_center[1] - R * mToPx(0.5))
+    cr.arc(far_side_rocket_center[0] + R * mToPx(0.866),
+           far_side_rocket_center[1] - R * mToPx(0.5), 5, 0, np.pi * 2.0)
+
+    #print(far_side_rocket_center)
+    near_side_rocket_center = [
+        X_BASE + mToPx((2.89973 + 3.15642) / 2.0), Y_BASE - mToPx(
+            (3.86305 + 3.39548) / 2.0)
+    ]
+    middle_rocket_center = [
+        X_BASE + mToPx((3.15642 + 3.6347) / 2.0), Y_BASE - mToPx(
+            (3.39548 + 3.392380) / 2.0)
+    ]
+    far_side_rocket_center = [
+        X_BASE + mToPx((3.63473 + 3.89984) / 2.0), Y_BASE - mToPx(
+            (3.39238 + 3.86305) / 2.0)
+    ]
+
+    cr.move_to(near_side_rocket_center[0], near_side_rocket_center[1])
+    cr.line_to(near_side_rocket_center[0] - 0.8 * mToPx(0.866),
+               near_side_rocket_center[1] + 0.8 * mToPx(0.5))
+
+    cr.move_to(middle_rocket_center[0], middle_rocket_center[1])
+    cr.line_to(middle_rocket_center[0], middle_rocket_center[1] + mToPx(0.8))
+
+    cr.move_to(far_side_rocket_center[0], far_side_rocket_center[1])
+    cr.line_to(far_side_rocket_center[0] + 0.8 * mToPx(0.866),
+               far_side_rocket_center[1] + 0.8 * mToPx(0.5))
+
+    # Leftmost Line
+    cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
+    cr.line_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
+
+    # Top Line
+    cr.move_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
+    cr.line_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
+
+    #Rightmost Line
+    cr.move_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
+    cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
+
+    #Back Line
+    cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
+    cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
+
+    # Bottom Rocket
+    # Leftmost Line
+    cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
+    cr.line_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
+
+    # Top Line
+    cr.move_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
+    cr.line_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
+
+    #Rightmost Line
+    cr.move_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
+    cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
+
+    #Back Line
+    cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
+    cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
+
+    cr.stroke()
+
+
+def draw_cargo_ship(cr):
+    # BASE Constants
+    X_BASE = 0 + mToPx(5.59435)
+    Y_BASE = 0 + 0  #mToPx(4.129151)
+    R = 0.381 - 0.1
+
+    FRONT_PEG_DELTA_Y = mToPx(0.276352)
+    cr.move_to(X_BASE, Y_BASE + FRONT_PEG_DELTA_Y)
+    cr.line_to(X_BASE - mToPx(0.8), Y_BASE + FRONT_PEG_DELTA_Y)
+
+    cr.move_to(X_BASE, Y_BASE + FRONT_PEG_DELTA_Y)
+    cr.arc(X_BASE - mToPx(R), Y_BASE + FRONT_PEG_DELTA_Y, 5, 0, np.pi * 2.0)
+
+    cr.move_to(X_BASE, Y_BASE - FRONT_PEG_DELTA_Y)
+    cr.line_to(X_BASE - mToPx(0.8), Y_BASE - FRONT_PEG_DELTA_Y)
+
+    cr.move_to(X_BASE, Y_BASE - FRONT_PEG_DELTA_Y)
+    cr.arc(X_BASE - mToPx(R), Y_BASE - FRONT_PEG_DELTA_Y, 5, 0, np.pi * 2.0)
+
+    SIDE_PEG_Y = mToPx(1.41605 / 2.0)
+    SIDE_PEG_X = X_BASE + mToPx(1.148842)
+    SIDE_PEG_DX = mToPx(0.55245)
+
+    cr.move_to(SIDE_PEG_X, SIDE_PEG_Y)
+    cr.line_to(SIDE_PEG_X, SIDE_PEG_Y + mToPx(0.8))
+    cr.move_to(SIDE_PEG_X, SIDE_PEG_Y + mToPx(R))
+    cr.arc(SIDE_PEG_X, SIDE_PEG_Y + mToPx(R), 5, 0, np.pi * 2.0)
+
+    cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y)
+    cr.line_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y + mToPx(0.8))
+    cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y + mToPx(R))
+    cr.arc(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y + mToPx(R), 5, 0, np.pi * 2.0)
+
+    cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y)
+    cr.line_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y + mToPx(0.8))
+    cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y + mToPx(R))
+    cr.arc(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y + mToPx(R), 5, 0,
+           np.pi * 2.0)
+
+    cr.move_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y)
+    cr.line_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y - mToPx(0.8))
+    cr.move_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y - mToPx(R))
+    cr.arc(SIDE_PEG_X, -1.0 * SIDE_PEG_Y - mToPx(R), 5, 0, np.pi * 2.0)
+
+    cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y)
+    cr.line_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(0.8))
+    cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(R))
+    cr.arc(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(R), 5, 0,
+           np.pi * 2.0)
+
+    cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y)
+    cr.line_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(0.8))
+    cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(R))
+    cr.arc(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(R), 5, 0,
+           np.pi * 2.0)
+
+    cr.rectangle(X_BASE, Y_BASE - mToPx(1.41605 / 2.0), mToPx(2.43205),
+                 mToPx(1.41605))
+    cr.stroke()
+
+
+def draw_points(cr, p, size):
+    for i in range(0, len(p)):
+        draw_px_cross(cr, p[i][0], p[i][1], size,
+                      Color(0, np.sqrt(0.2 * i), 0))
diff --git a/frc971/control_loops/python/graph.py b/frc971/control_loops/python/graph.py
new file mode 100644
index 0000000..7e98a38
--- /dev/null
+++ b/frc971/control_loops/python/graph.py
@@ -0,0 +1,178 @@
+from constants import *
+import cairo
+from color import Color, palette
+from points import Points
+from drawing_constants import *
+from libspline import Spline, DistanceSpline, Trajectory
+
+AXIS_MARGIN_SPACE = 40
+
+
+class Graph():  # (TODO): Remove Computer Calculation
+    def __init__(self, cr, mypoints):
+        # Background Box
+        set_color(cr, palette["WHITE"])
+        cr.rectangle(-1.0 * SCREEN_SIZE, -0.5 * SCREEN_SIZE, SCREEN_SIZE,
+                     SCREEN_SIZE * 0.6)
+        cr.fill()
+
+        cr.set_source_rgb(0, 0, 0)
+        cr.rectangle(-1.0 * SCREEN_SIZE, -0.5 * SCREEN_SIZE, SCREEN_SIZE,
+                     SCREEN_SIZE * 0.6)
+        #Axis
+        cr.move_to(-1.0 * SCREEN_SIZE + AXIS_MARGIN_SPACE,
+                   -0.5 * SCREEN_SIZE + AXIS_MARGIN_SPACE)  # Y
+        cr.line_to(-1.0 * SCREEN_SIZE + AXIS_MARGIN_SPACE,
+                   0.1 * SCREEN_SIZE - 10)
+
+        cr.move_to(-1.0 * SCREEN_SIZE + AXIS_MARGIN_SPACE,
+                   -0.5 * SCREEN_SIZE + AXIS_MARGIN_SPACE)  # X
+        cr.line_to(-10, -0.5 * SCREEN_SIZE + AXIS_MARGIN_SPACE)
+        cr.stroke()
+
+        skip = 2
+        dT = 0.00505
+        start = AXIS_MARGIN_SPACE - SCREEN_SIZE
+        end = -2.0 * AXIS_MARGIN_SPACE
+        height = 0.5 * (SCREEN_SIZE) - AXIS_MARGIN_SPACE
+        zero = AXIS_MARGIN_SPACE - SCREEN_SIZE / 2.0
+        if mypoints.getLibsplines():
+            distanceSpline = DistanceSpline(mypoints.getLibsplines())
+            traj = Trajectory(distanceSpline)
+            traj.Plan()
+            XVA = traj.GetPlanXVA(dT)
+            self.draw_x_axis(cr, start, height, zero, XVA, end)
+            self.drawVelocity(cr, XVA, start, height, skip, zero, end)
+            self.drawAcceleration(cr, XVA, start, height, skip, zero,
+                                  AXIS_MARGIN_SPACE, end)
+            self.drawVoltage(cr, XVA, start, height, skip, traj, zero, end)
+            cr.set_source_rgb(0, 0, 0)
+            cr.move_to(-1.0 * AXIS_MARGIN_SPACE, zero + height / 2.0)
+            cr.line_to(AXIS_MARGIN_SPACE - SCREEN_SIZE, zero + height / 2.0)
+        cr.stroke()
+
+    def connectLines(self, cr, points, color):
+        for i in range(0, len(points) - 1):
+            set_color(cr, color)
+            cr.move_to(points[i][0], points[i][1])
+            cr.line_to(points[i + 1][0], points[i + 1][1])
+            cr.stroke()
+
+    def draw_x_axis(self, cr, start, height, zero, xva, end):
+        total_time = 0.00505 * len(xva[0])
+        for k in np.linspace(0, 1, 11):
+            self.tickMark(cr,
+                          k * np.abs(start - end) + start, zero + height / 2.0,
+                          10, palette["BLACK"])
+            cr.move_to(k * np.abs(start - end) + start,
+                       10 + zero + height / 2.0)
+            txt_scale = SCREEN_SIZE / 1000.0
+            display_text(cr, str(round(k * total_time, 3)), txt_scale,
+                         txt_scale, 1.0 / txt_scale, 1.0 / txt_scale)
+            cr.stroke()
+
+    def tickMark(self, cr, x, y, height, COLOR):
+        # X, Y is in the middle of the tick mark
+        set_color(cr, COLOR)
+        cr.move_to(x, y + (height / 2))
+        cr.line_to(x, y - (height / 2))
+        cr.stroke()
+
+    def HtickMark(self, cr, x, y, width, COLOR):
+        # X, Y is in the middle of the tick mark
+        set_color(cr, COLOR)
+        cr.move_to(x + (width / 2), y)
+        cr.line_to(x - (width / 2), y)
+        cr.stroke()
+
+    def drawVelocity(self, cr, xva, start, height, skip, zero, end):
+        COLOR = palette["RED"]
+        velocity = xva[1]
+        n_timesteps = len(velocity)
+        max_v = np.amax(velocity)
+        spacing = np.abs(start - end) / float(n_timesteps)
+        scaler = height / max_v
+        cr.set_source_rgb(1, 0, 0)
+        points = []
+        for i in range(0, len(velocity)):
+            if i % skip == 0:
+                points.append([
+                    start + (i * spacing),
+                    zero + height / 2.0 + (velocity[i] * scaler / 2.0)
+                ])
+        self.connectLines(cr, points, COLOR)
+
+        # draw axes marking
+        for i in np.linspace(-1, 1, 11):
+            self.HtickMark(cr, start, zero + i * height / 2.0 + height / 2.0,
+                           10, palette["BLACK"])
+            cr.set_source_rgb(1, 0, 0)
+            cr.move_to(start + 5, zero + i * height / 2.0 + height / 2.0)
+            txt_scale = SCREEN_SIZE / 1000.0
+            display_text(cr, str(round(i * max_v, 2)), txt_scale, txt_scale,
+                         1.0 / txt_scale, 1.0 / txt_scale)
+            cr.stroke()
+
+    def drawAcceleration(self, cr, xva, start, height, skip, zero, margin,
+                         end):
+        COLOR = palette["BLUE"]
+        accel = xva[2]
+        max_a = np.amax(accel)
+        min_a = np.amin(accel)
+        n_timesteps = len(accel)
+        spacing = np.abs(start - end) / float(n_timesteps)
+        scaler = height / (max_a - min_a)
+        cr.set_source_rgb(1, 0, 0)
+        points = []
+        for i in range(0, len(accel)):
+            if i % skip == 0:
+                points.append([
+                    start + (i * spacing), zero + ((accel[i] - min_a) * scaler)
+                ])
+        self.connectLines(cr, points, COLOR)
+
+        # draw axes marking
+        for i in np.linspace(0, 1, 11):
+            self.HtickMark(cr, -1.5 * margin, zero + i * height, 10,
+                           palette["BLACK"])
+            cr.set_source_rgb(0, 0, 1)
+            cr.move_to(-1.2 * margin, zero + i * height)
+            txt_scale = SCREEN_SIZE / 1000.0
+            display_text(cr, str(round(i * (max_a - min_a) + min_a,
+                                       2)), txt_scale, txt_scale,
+                         1.0 / txt_scale, 1.0 / txt_scale)
+            cr.stroke()
+
+    def drawVoltage(self, cr, xva, start, height, skip, traj, zero, end):
+        COLOR1 = palette["GREEN"]
+        COLOR2 = palette["CYAN"]
+        poses = xva[0]
+        n_timesteps = len(poses)
+        spacing = np.abs(start - end) / float(n_timesteps)
+        points1 = []
+        points2 = []
+        for i in range(0, len(poses)):
+            if i % skip == 0:
+                voltage = traj.Voltage(poses[i])
+                points1.append([
+                    start + (i * spacing),
+                    zero + height / 2 + height * (voltage[0] / 24.0)
+                ])
+                points2.append([
+                    start + (i * spacing),
+                    zero + height / 2 + height * (voltage[1] / 24.0)
+                ])
+        self.connectLines(cr, points1, COLOR1)
+        self.connectLines(cr, points2, COLOR2)
+
+        for i in np.linspace(-1, 1, 7):
+            self.HtickMark(cr, -1.0 * SCREEN_SIZE,
+                           zero + i * height / 2.0 + height / 2.0, 10,
+                           palette["BLACK"])
+            cr.set_source_rgb(0, 1, 1)
+            cr.move_to(-1.0 * SCREEN_SIZE,
+                       zero + i * height / 2.0 + height / 2.0)
+            txt_scale = SCREEN_SIZE / 1000.0
+            display_text(cr, str(round(i * 12.0, 2)), txt_scale, txt_scale,
+                         1.0 / txt_scale, 1.0 / txt_scale)
+            cr.stroke()
diff --git a/frc971/control_loops/python/libspline.py b/frc971/control_loops/python/libspline.py
old mode 100644
new mode 100755
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
old mode 100644
new mode 100755
index e59286f..0d1b263
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -3,67 +3,23 @@
 import os
 import sys
 import copy
-import basic_window
 from color import Color, palette
 import random
 import gi
 import numpy as np
-from libspline import Spline
 import scipy.spatial.distance
 gi.require_version('Gtk', '3.0')
+gi.require_version('Gdk', '3.0')
 from gi.repository import Gdk, Gtk, GLib
 import cairo
+from libspline import Spline, DistanceSpline, Trajectory
 import enum
-import json # For writing to json files
-
-from basic_window import OverrideMatrix, identity, quit_main_loop, set_color
-
-WIDTH_OF_FIELD_IN_METERS = 8.258302
-PIXELS_ON_SCREEN = 300
-
-
-def pxToM(p):
-    return p * WIDTH_OF_FIELD_IN_METERS / PIXELS_ON_SCREEN
-
-
-def mToPx(m):
-    return (m*PIXELS_ON_SCREEN/WIDTH_OF_FIELD_IN_METERS)
-
-def px(cr):
-    return OverrideMatrix(cr, identity)
-
-
-def draw_px_cross(cr, x, y, length_px, color=palette["RED"]):
-    """Draws a cross with fixed dimensions in pixel space."""
-    set_color(cr, color)
-    cr.move_to(x, y - length_px)
-    cr.line_to(x, y + length_px)
-    cr.stroke()
-
-    cr.move_to(x - length_px, y)
-    cr.line_to(x + length_px, y)
-    cr.stroke()
-    set_color(cr, palette["LIGHT_GREY"])
-
-
-def draw_px_x(cr, x, y, length_px1, color=palette["BLACK"]):
-    """Draws a x with fixed dimensions in pixel space."""
-    length_px = length_px1 / np.sqrt(2)
-    set_color(cr, color)
-    cr.move_to(x - length_px, y - length_px)
-    cr.line_to(x + length_px, y + length_px)
-    cr.stroke()
-
-    cr.move_to(x - length_px, y + length_px)
-    cr.line_to(x + length_px, y - length_px)
-    cr.stroke()
-    set_color(cr, palette["LIGHT_GREY"])
-
-
-def draw_points(cr, p, size):
-    for i in range(0, len(p)):
-        draw_px_cross(cr, p[i][0], p[i][1], size, Color(
-            0, np.sqrt(0.2 * i), 0))
+import json
+from basic_window import *
+from constants import *
+from drawing_constants import *
+from points import Points
+from graph import Graph
 
 
 class Mode(enum.Enum):
@@ -72,61 +28,25 @@
     kEditing = 2
     kExporting = 3
     kImporting = 4
-    kConstraint = 5
 
 
-class ConstraintType(enum.Enum):
-    kMaxSpeed = 0
-    kMaxAcceleration = 1
-
-
-def display_text(cr, text, widtha, heighta, widthb, heightb):
-    cr.scale(widtha, -heighta)
-    cr.show_text(text)
-    cr.scale(widthb, -heightb)
-
-
-def redraw(needs_redraw, window):
-    print("Redrew")
-    if not needs_redraw:
-        window.queue_draw()
-
-
-class Constraint():
-    def __init__(self, start, end, constraint, value):
-        self.start = start  #Array with index and distance from start of spline
-        self.end = end  #Array with index and distance from start of spline
-        self.constraint = constraint  #INT
-        self.value = value  #INT
-        if self.constraint == 0:
-            self.conName = "kMaxSpeed"
-        else:
-            self.conName = "kMaxAcceleration"
-
-    def toString(self):
-
-        return "START: " + str(self.start[0]) + ",   " + str(
-            self.start[1]) + " |  END: " + str(self.end[0]) + ",   " + str(
-                self.end[1]) + " |  " + str(self.conName) + ":  " + str(
-                    self.value)
-
-
-class GTK_Widget(basic_window.BaseWindow):
+class GTK_Widget(BaseWindow):
     """Create a GTK+ widget on which we will draw using Cairo"""
-
     def __init__(self):
         super(GTK_Widget, self).__init__()
 
+        self.points = Points()
+
         # init field drawing
         # add default spline for testing purposes
         # init editing / viewing modes and pointer location
         self.mode = Mode.kPlacing
         self.x = 0
         self.y = 0
-
         module_path = os.path.dirname(os.path.realpath(sys.argv[0]))
         self.path_to_export = os.path.join(module_path,
-            'points_for_pathedit.json')
+                                           'points_for_pathedit.json')
+
         # update list of control points
         self.point_selected = False
         # self.adding_spline = False
@@ -145,32 +65,20 @@
         for c in palette:
             self.colors.append(palette[c])
 
-        self.selected_points = []
-        self.splines = []
         self.reinit_extents()
 
         self.inStart = None
         self.inEnd = None
-        self.inConstraint = None
         self.inValue = None
         self.startSet = False
 
-    #John also wrote this
-    def add_point(self, x, y):
-        if (len(self.selected_points) < 6):
-            self.selected_points.append([pxToM(x), pxToM(y)])
-            if (len(self.selected_points) == 6):
-                self.mode = Mode.kEditing
-                self.splines.append(np.array(self.selected_points))
-                self.selected_points = []
-
     """set extents on images"""
 
     def reinit_extents(self):
-        self.extents_x_min = -800
-        self.extents_x_max = 800
-        self.extents_y_min = -800
-        self.extents_y_max = 800
+        self.extents_x_min = -1.0 * SCREEN_SIZE
+        self.extents_x_max = SCREEN_SIZE
+        self.extents_y_min = -1.0 * SCREEN_SIZE
+        self.extents_y_max = SCREEN_SIZE
 
     # this needs to be rewritten with numpy, i dont think this ought to have
     # SciPy as a dependecy
@@ -184,192 +92,112 @@
     def get_nearest_point(self):
         return self.all_controls[self.get_index_of_nearest_point()]
 
-    def set_index_to_nearest_spline_point(self):
-        nearest = 50
-        index_of_closest = 0
-        self.spline_edit = 0
-        cur_p = [self.x, self.y]
-
-        for index_splines, points in enumerate(self.spline):
-            for index_points, point in enumerate(points.curve):
-                # pythagorean
-                distance = np.sqrt((cur_p[0] - mToPx(point[0]))**2 +
-                                   (cur_p[1] - mToPx(point[1])**2))
-                if distance < nearest:
-                    nearest = distance
-                    print("DISTANCE: ", distance, " | INDEX: ", index_points)
-                    index_of_closest = index_points
-                    self.index_of_edit = index_of_closest
-                    self.spline_edit = index_splines
-                    self.held_x = self.x
-        if self.startSet == False:
-            self.inStart = [self.index_of_edit, self.findDistance()]
-            self.startSet = True
-        else:
-            self.inEnd = [self.index_of_edit, self.findDistance()]
-            self.startSet = False
-            self.mode = Mode.kEditing
-            self.spline_edit = -1
-            self.index_of_edit = -1
-
-        print("Nearest: " + str(nearest))
-        print("Spline: " + str(self.spline_edit))
-        print("Index: " + str(index_of_closest))
-
-    def findDistance(self):
-        """ findDistance goes through each point on the spline finding distance to that point from the point before.
-        It does this to find the the length of the spline to the point that is currently selected.
-        """
-        distance = 0
-        points = self.curves[self.spline_edit]
-        for index, point in enumerate(points):
-            if index > 0 and index <= self.index_of_edit:
-                distance += np.sqrt((points[index - 1][0] - point[0])**2 +
-                                    (points[index - 1][1] - point[1])**2)
-        return distance
-
-    def draw_HAB(self, cr):
-        print("WENT IN")
-        # BASE Constants
-        X_BASE = -450+mToPx(2.41568)
-        Y_BASE  = -150+mToPx(4.129151)
-
-        BACKWALL_X = -450
-
-        # HAB Levels 2 and 3 called in variables backhab
-
-        WIDTH_BACKHAB = mToPx(1.2192)
-
-        Y_TOP_BACKHAB_BOX = Y_BASE + mToPx(0.6096)
-        BACKHAB_LV2_LENGTH = mToPx(1.016)
-
-        BACKHAB_LV3_LENGTH = mToPx(1.2192)
-        Y_LV3_BOX = Y_TOP_BACKHAB_BOX - BACKHAB_LV3_LENGTH
-
-        Y_BOTTOM_BACKHAB_BOX = Y_LV3_BOX -BACKHAB_LV2_LENGTH
-
-        # HAB LEVEL 1
-        X_LV1_BOX = BACKWALL_X + WIDTH_BACKHAB
-
-        WIDTH_LV1_BOX = mToPx(0.90805)
-        LENGTH_LV1_BOX = mToPx(1.6256)
-
-        Y_BOTTOM_LV1_BOX = Y_BASE - LENGTH_LV1_BOX
-
-        # Ramp off Level 1
-        X_RAMP = X_LV1_BOX
-
-        Y_TOP_RAMP = Y_BASE + LENGTH_LV1_BOX
-        WIDTH_TOP_RAMP = mToPx(1.20015)
-        LENGTH_TOP_RAMP = Y_BASE + mToPx(0.28306)
-
-        X_MIDDLE_RAMP = X_RAMP + WIDTH_LV1_BOX
-        Y_MIDDLE_RAMP = Y_BOTTOM_LV1_BOX
-        LENGTH_MIDDLE_RAMP = 2*LENGTH_LV1_BOX
-        WIDTH_MIDDLE_RAMP = WIDTH_TOP_RAMP - WIDTH_LV1_BOX
-
-        Y_BOTTOM_RAMP = Y_BASE - LENGTH_LV1_BOX - LENGTH_TOP_RAMP
-
-        # Side Bars to Hold in balls
-        X_BARS = BACKWALL_X
-        WIDTH_BARS = WIDTH_BACKHAB
-        LENGTH_BARS = mToPx(0.574675)
-
-        Y_TOP_BAR = Y_TOP_BACKHAB_BOX + BACKHAB_LV2_LENGTH
-
-        Y_BOTTOM_BAR = Y_BOTTOM_BACKHAB_BOX - LENGTH_BARS
-
-        set_color(cr, palette["BLACK"])
-        cr.rectangle(BACKWALL_X, Y_TOP_BACKHAB_BOX, WIDTH_BACKHAB,
-                BACKHAB_LV2_LENGTH)
-        cr.rectangle(BACKWALL_X, Y_LV3_BOX, WIDTH_BACKHAB,
-                BACKHAB_LV3_LENGTH)
-        cr.rectangle(BACKWALL_X, Y_BOTTOM_BACKHAB_BOX, WIDTH_BACKHAB,
-                BACKHAB_LV2_LENGTH)
-        cr.rectangle(X_LV1_BOX, Y_BASE, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
-        cr.rectangle(X_LV1_BOX, Y_BOTTOM_LV1_BOX, WIDTH_LV1_BOX,
-                LENGTH_LV1_BOX)
-        cr.rectangle(X_RAMP, Y_TOP_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
-        cr.rectangle(X_MIDDLE_RAMP, Y_MIDDLE_RAMP, WIDTH_MIDDLE_RAMP,
-                LENGTH_MIDDLE_RAMP)
-        cr.rectangle(X_RAMP, Y_BOTTOM_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
-        cr.rectangle(X_BARS, Y_TOP_BAR, WIDTH_BARS, LENGTH_BARS)
-        cr.rectangle(X_BARS, Y_BOTTOM_BAR, WIDTH_BARS, LENGTH_BARS)
-        cr.stroke()
-        #draw_px_x(cr, BACKWALL_X, 0, 10) # Midline Point
-        #draw_px_x(cr, X_BASE, Y_BASE, 10) # Bases
-        cr.set_line_join(cairo.LINE_JOIN_ROUND)
-
-        cr.stroke()
-
-    def draw_rockets(self, cr):
-        # BASE Constants
-        X_BASE = -450+mToPx(2.41568)
-        Y_BASE  = -150+mToPx(4.129151)
-
-        # Top Rocket
-
-        # Leftmost Line
-        cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
-        cr.line_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
-
-        # Top Line
-        cr.move_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
-        cr.line_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
-
-        #Rightmost Line
-        cr.move_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
-        cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
-
-        #Back Line
-        cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
-        cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
-
-        # Bottom Rocket
-
-        # Leftmost Line
-        cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
-        cr.line_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
-
-        # Top Line
-        cr.move_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
-        cr.line_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
-
-        #Rightmost Line
-        cr.move_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
-        cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
-
-        #Back Line
-        cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
-        cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
-
-        cr.stroke()
-
-    def draw_cargo_ship(self, cr):
-        # BASE Constants
-        X_BASE = -450+mToPx(2.41568)
-        Y_BASE  = -150+mToPx(4.129151)
-
-        cr.rectangle(X_BASE + mToPx(3.15912), Y_BASE - mToPx(0.72326),
-                mToPx(2.43205), mToPx(1.41605))
-
-        cr.stroke()
-
     def draw_field_elements(self, cr):
-        self.draw_HAB(cr)
-        self.draw_rockets(cr)
-        self.draw_cargo_ship(cr)
+        draw_HAB(cr)
+        draw_rockets(cr)
+        draw_cargo_ship(cr)
 
-    def handle_draw(self, cr):
-        # print(self.new_point)
-        # print("SELF.POINT_SELECTED: " + str(self.point_selected))
+    def draw_robot_at_point(self, cr, i, p, spline):
+        p1 = [mToPx(spline.Point(i)[0]), mToPx(spline.Point(i)[1])]
+        p2 = [mToPx(spline.Point(i + p)[0]), mToPx(spline.Point(i + p)[1])]
 
-        # begin drawing
+        #Calculate Robot
+        distance = np.sqrt((p2[1] - p1[1])**2 + (p2[0] - p1[0])**2)
+        x_difference_o = p2[0] - p1[0]
+        y_difference_o = p2[1] - p1[1]
+        x_difference = x_difference_o * mToPx(LENGTH_OF_ROBOT / 2) / distance
+        y_difference = y_difference_o * mToPx(LENGTH_OF_ROBOT / 2) / distance
+
+        front_middle = []
+        front_middle.append(p1[0] + x_difference)
+        front_middle.append(p1[1] + y_difference)
+
+        back_middle = []
+        back_middle.append(p1[0] - x_difference)
+        back_middle.append(p1[1] - y_difference)
+
+        slope = [-(1 / x_difference_o) / (1 / y_difference_o)]
+        angle = np.arctan(slope)
+
+        x_difference = np.sin(angle[0]) * mToPx(WIDTH_OF_ROBOT / 2)
+        y_difference = np.cos(angle[0]) * mToPx(WIDTH_OF_ROBOT / 2)
+
+        front_1 = []
+        front_1.append(front_middle[0] - x_difference)
+        front_1.append(front_middle[1] - y_difference)
+
+        front_2 = []
+        front_2.append(front_middle[0] + x_difference)
+        front_2.append(front_middle[1] + y_difference)
+
+        back_1 = []
+        back_1.append(back_middle[0] - x_difference)
+        back_1.append(back_middle[1] - y_difference)
+
+        back_2 = []
+        back_2.append(back_middle[0] + x_difference)
+        back_2.append(back_middle[1] + y_difference)
+
+        x_difference = x_difference_o * mToPx(
+            LENGTH_OF_ROBOT / 2 + ROBOT_SIDE_TO_BALL_CENTER) / distance
+        y_difference = y_difference_o * mToPx(
+            LENGTH_OF_ROBOT / 2 + ROBOT_SIDE_TO_BALL_CENTER) / distance
+
+        #Calculate Ball
+        ball_center = []
+        ball_center.append(p1[0] + x_difference)
+        ball_center.append(p1[1] + y_difference)
+
+        x_difference = x_difference_o * mToPx(
+            LENGTH_OF_ROBOT / 2 + ROBOT_SIDE_TO_HATCH_PANEL) / distance
+        y_difference = y_difference_o * mToPx(
+            LENGTH_OF_ROBOT / 2 + ROBOT_SIDE_TO_HATCH_PANEL) / distance
+
+        #Calculate Panel
+        panel_center = []
+        panel_center.append(p1[0] + x_difference)
+        panel_center.append(p1[1] + y_difference)
+
+        x_difference = np.sin(angle[0]) * mToPx(HATCH_PANEL_WIDTH / 2)
+        y_difference = np.cos(angle[0]) * mToPx(HATCH_PANEL_WIDTH / 2)
+
+        panel_1 = []
+        panel_1.append(panel_center[0] + x_difference)
+        panel_1.append(panel_center[1] + y_difference)
+
+        panel_2 = []
+        panel_2.append(panel_center[0] - x_difference)
+        panel_2.append(panel_center[1] - y_difference)
+
+        #Draw Robot
+        cr.move_to(front_1[0], front_1[1])
+        cr.line_to(back_1[0], back_1[1])
+        cr.line_to(back_2[0], back_2[1])
+        cr.line_to(front_2[0], front_2[1])
+        cr.line_to(front_1[0], front_1[1])
+
+        cr.stroke()
+
+        #Draw Ball
+        set_color(cr, palette["ORANGE"], 0.5)
+        cr.move_to(back_middle[0], back_middle[1])
+        cr.line_to(ball_center[0], ball_center[1])
+        cr.arc(ball_center[0], ball_center[1], mToPx(BALL_RADIUS), 0,
+               2 * np.pi)
+        cr.stroke()
+
+        #Draw Panel
+        set_color(cr, palette["YELLOW"], 0.5)
+        cr.move_to(panel_1[0], panel_1[1])
+        cr.line_to(panel_2[0], panel_2[1])
+
+        cr.stroke()
+        cr.set_source_rgba(0, 0, 0, 1)
+
+    def handle_draw(self, cr):  # main
         # Fill the background color of the window with grey
-        set_color(cr, palette["GREY"])
+        set_color(cr, palette["WHITE"])
         cr.paint()
-        #Scale the field to fit within drawing area
-        cr.scale(0.5, 0.5)
 
         # Draw a extents rectangle
         set_color(cr, palette["WHITE"])
@@ -378,209 +206,181 @@
                      self.extents_y_max - self.extents_y_min)
         cr.fill()
 
-        #Drawing the field
+        #Drawing the switch and scale in the field
         cr.move_to(0, 50)
         cr.show_text('Press "e" to export')
         cr.show_text('Press "i" to import')
 
-        set_color(cr, Color(0.3, 0.3, 0.3))
-        cr.rectangle(-450, -150, 300, 300)
+        set_color(cr, palette["WHITE"])
+        cr.rectangle(0, -mToPx(8.2296 / 2.0), SCREEN_SIZE, SCREEN_SIZE)
         cr.fill()
         set_color(cr, palette["BLACK"])
-        cr.rectangle(-450, -150, 300, 300)
+        cr.rectangle(0, -mToPx(8.2296 / 2.0), SCREEN_SIZE, SCREEN_SIZE)
         cr.set_line_join(cairo.LINE_JOIN_ROUND)
         cr.stroke()
-
         self.draw_field_elements(cr)
-
         y = 0
 
-        # update all the things
+        # update everything
 
-        if self.mode == Mode.kViewing:
+        if self.mode == Mode.kPlacing or self.mode == Mode.kViewing:
             set_color(cr, palette["BLACK"])
-            cr.move_to(-300, 170)
-            cr.show_text("VIEWING")
-            set_color(cr, palette["GREY"])
-
-            if len(self.selected_points) > 0:
-                print("SELECTED_POINTS: " + str(len(self.selected_points)))
-                print("ITEMS:")
-                # for item in self.selected_points:
-                # print(str(item))
-                for i, point in enumerate(self.selected_points):
-                    # print("I: " + str(i))
-                    draw_px_x(cr, point[0], point[1], 10)
-                    cr.move_to(point[0], point[1] - 15)
-                    display_text(cr, str(i), 0.5, 0.5, 2, 2)
-
-        elif self.mode == Mode.kPlacing:
-            set_color(cr, palette["BLACK"])
-            cr.move_to(-300, 170)
-            display_text(cr, "ADD", 1, 1, 1, 1)
-            set_color(cr, palette["GREY"])
-
-            if len(self.selected_points) > 0:
-                print("SELECTED_POINTS: " + str(len(self.selected_points)))
-                print("ITEMS:")
-                for item in self.selected_points:
-                    print(str(item))
-                for i, point in enumerate(self.selected_points):
-                    print("I: " + str(i))
+            cr.move_to(-SCREEN_SIZE, 170)
+            plotPoints = self.points.getPoints()
+            if plotPoints:
+                for i, point in enumerate(plotPoints):
                     draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 10)
                     cr.move_to(mToPx(point[0]), mToPx(point[1]) - 15)
                     display_text(cr, str(i), 0.5, 0.5, 2, 2)
+            set_color(cr, palette["WHITE"])
 
         elif self.mode == Mode.kEditing:
             set_color(cr, palette["BLACK"])
-            cr.move_to(-300, 170)
+            cr.move_to(-SCREEN_SIZE, 170)
             display_text(cr, "EDITING", 1, 1, 1, 1)
-            if len(self.splines) > 0:
-                holder_spline = []
-                for i, points in enumerate(self.splines):
-                    array = np.zeros(shape=(6, 2), dtype=float)
-                    for j, point in enumerate(points):
-                        array[j, 0] = mToPx(point[0])
-                        array[j, 1] = mToPx(point[1])
-                    spline = Spline(np.ascontiguousarray(np.transpose(array)))
-                    for k in np.linspace(0.01, 1, 100):
-                        cr.move_to(
-                            spline.Point(k - 0.01)[0],
-                            spline.Point(k - 0.01)[1])
-                        cr.line_to(spline.Point(k)[0], spline.Point(k)[1])
-                        cr.stroke()
-                        holding = [
-                            spline.Point(k - 0.01)[0],
-                            spline.Point(k - 0.01)[1]
-                        ]
-                        holder_spline.append(holding)
-                self.curves.append(holder_spline)
+            if self.points.getSplines():
+                self.draw_splines(cr)
+                for i, points in enumerate(self.points.getSplines()):
 
-                for spline, points in enumerate(self.splines):
-                    # for item in points:
-                    #     print(str(item))
-                    for i, point in enumerate(points):
-                        # print("I: " + str(i))
-                        if spline == self.spline_edit and i == self.index_of_edit:
-                            draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 15,
-                                      self.colors[spline])
-                        elif (spline == 0 and not i == 5) or (not i == 0
-                                                              and not i == 5):
-                            draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 10,
-                                      self.colors[spline])
-                        cr.move_to(mToPx(point[0]), mToPx(point[1]) - 15)
-                        display_text(cr, str(i), 0.5, 0.5, 2, 2)
+                    p0 = np.array([mToPx(points[0][0]), mToPx(points[0][1])])
+                    p1 = np.array([mToPx(points[1][0]), mToPx(points[1][1])])
+                    p2 = np.array([mToPx(points[2][0]), mToPx(points[2][1])])
+                    p3 = np.array([mToPx(points[3][0]), mToPx(points[3][1])])
+                    p4 = np.array([mToPx(points[4][0]), mToPx(points[4][1])])
+                    p5 = np.array([mToPx(points[5][0]), mToPx(points[5][1])])
 
-        elif self.mode == Mode.kConstraint:
-            print("Drawn")
-            set_color(cr, palette["BLACK"])
-            cr.move_to(-300, 170)
-            display_text(cr, "Adding Constraint", 1, 1, 1, 1)
-            if len(self.splines) > 0:
-                # print("Splines: " + str(len(self.splines)))
-                # print("ITEMS:")
-                for s, points in enumerate(self.splines):
-                    # for item in points:
-                    #     print(str(item))
-                    for i, point in enumerate(points):
-                        # print("I: " + str(i))
-                        draw_px_x(cr, point[0], point[1], 10, self.colors[s])
-                        cr.move_to(point[0], point[1] - 15)
-                        display_text(cr, str(i), 0.5, 0.5, 2, 2)
+                    draw_control_points(cr, [p0, p1, p2, p3, p4, p5])
+                    first_tangent = p0 + 2.0 * (p1 - p0)
+                    second_tangent = p5 + 2.0 * (p4 - p5)
+                    cr.set_source_rgb(0, 0.5, 0)
+                    cr.move_to(p0[0], p0[1])
+                    cr.set_line_width(1.0)
+                    cr.line_to(first_tangent[0], first_tangent[1])
+                    cr.move_to(first_tangent[0], first_tangent[1])
+                    cr.line_to(p2[0], p2[1])
 
-        cr.paint_with_alpha(.65)
+                    cr.move_to(p5[0], p5[1])
+                    cr.line_to(second_tangent[0], second_tangent[1])
 
+                    cr.move_to(second_tangent[0], second_tangent[1])
+                    cr.line_to(p3[0], p3[1])
+
+                    cr.stroke()
+                    cr.set_line_width(2.0)
+            self.points.update_lib_spline()
+            set_color(cr, palette["WHITE"])
+
+        cr.paint_with_alpha(0.2)
+
+        mygraph = Graph(cr, self.points)
         draw_px_cross(cr, self.x, self.y, 10)
 
-    def do_key_press(self, event):
+    def draw_splines(self, cr):
+        holder_spline = []
+        for i, points in enumerate(self.points.getSplines()):
+            array = np.zeros(shape=(6, 2), dtype=float)
+            for j, point in enumerate(points):
+                array[j, 0] = point[0]
+                array[j, 1] = point[1]
+            spline = Spline(np.ascontiguousarray(np.transpose(array)))
+            for k in np.linspace(0.01, 1, 100):
+                cr.move_to(mToPx(spline.Point(k - 0.01)[0]),
+                           mToPx(spline.Point(k - 0.01)[1]))
+                cr.line_to(mToPx(spline.Point(k)[0]),
+                           mToPx(spline.Point(k)[1]))
+                cr.stroke()
+                holding = [
+                    spline.Point(k - 0.01)[0],
+                    spline.Point(k - 0.01)[1]
+                ]
+                holder_spline.append(holding)
+            if i == 0:
+                self.draw_robot_at_point(cr, 0.00, 0.01, spline)
+            self.draw_robot_at_point(cr, 1, 0.01, spline)
+        self.curves.append(holder_spline)
+
+    def mouse_move(self, event):
+        old_x = self.x
+        old_y = self.y
+        self.x = event.x
+        self.y = event.y
+        dif_x = event.x - old_x
+        dif_y = event.y - old_y
+        difs = np.array([pxToM(dif_x), pxToM(dif_y)])
+
+        if self.mode == Mode.kEditing:
+            self.spline_edit = self.points.updates_for_mouse_move(
+                self.index_of_edit, self.spline_edit, self.x, self.y, difs)
+
+    def do_key_press(self, event, file_name):
         keyval = Gdk.keyval_to_lower(event.keyval)
-        # print("Gdk.KEY_" + Gdk.keyval_name(keyval))
+        module_path = os.path.dirname(os.path.realpath(sys.argv[0]))
+        self.path_to_export = os.path.join(module_path,
+                                           "spline_jsons/" + file_name)
         if keyval == Gdk.KEY_q:
             print("Found q key and exiting.")
             quit_main_loop()
-        if keyval == Gdk.KEY_e:
-            # Will export to json file
-            self.mode = Mode.kEditing
-            print(str(sys.argv))
-            print('out to: ', self.path_to_export)
-            exportList = [l.tolist() for l in self.splines]
-            with open(self.path_to_export, mode='w') as points_file:
-                json.dump(exportList, points_file)
-                print("Wrote: " + str(self.splines))
-        if keyval == Gdk.KEY_i:
-            # import from json file
-            self.mode = Mode.kEditing
-            self.selected_points = []
-            self.splines = []
-            with open(self.path_to_export) as points_file:
-                self.splines = json.load(points_file)
-                print("Added: " + str(self.splines))
+        file_name_end = file_name[-5:]
+        if file_name_end != ".json":
+            print("Error: Filename doesn't end in .json")
+        else:
+            if keyval == Gdk.KEY_e:
+                # Will export to json file
+                self.mode = Mode.kEditing
+                print('out to: ', self.path_to_export)
+                exportList = [l.tolist() for l in self.points.getSplines()]
+                with open(self.path_to_export, mode='w') as points_file:
+                    json.dump(exportList, points_file)
+
+            if keyval == Gdk.KEY_i:
+                # import from json file
+                self.mode = Mode.kEditing
+                self.points.resetPoints()
+                self.points.resetSplines()
+                with open(self.path_to_export) as points_file:
+                    self.points.setUpSplines(json.load(points_file))
+
+                self.points.update_lib_spline()
+
         if keyval == Gdk.KEY_p:
             self.mode = Mode.kPlacing
             # F0 = A1
             # B1 = 2F0 - E0
             # C1= d0 + 4F0 - 4E0
-            spline_index = len(self.splines) - 1
-            self.selected_points = []
-            f = self.splines[spline_index][5]
-            e = self.splines[spline_index][4]
-            d = self.splines[spline_index][3]
-            self.selected_points.append(f)
-            self.selected_points.append(f * 2 + e * -1)
-            self.selected_points.append(d + f * 4 + e * -4)
-
-        if keyval == Gdk.KEY_c:
-            self.mode = Mode.kConstraint
+            spline_index = len(self.points.getSplines()) - 1
+            self.points.resetPoints()
+            self.points.extrapolate(
+                self.points.getSplines()[len(self.points.getSplines()) - 1][5],
+                self.points.getSplines()[len(self.points.getSplines()) - 1][4],
+                self.points.getSplines()[len(self.points.getSplines()) - 1][3])
 
     def button_press_action(self):
         if self.mode == Mode.kPlacing:
-            #Check that the point clicked is on the field
-            if (self.x < -150 and self.x > -450 and self.y < 150
-                    and self.y > -150):
-                self.add_point(self.x, self.y)
+            if self.points.add_point(self.x, self.y):
+                self.mode = Mode.kEditing
         elif self.mode == Mode.kEditing:
             # Now after index_of_edit is not -1, the point is selected, so
             # user can click for new point
             if self.index_of_edit > -1 and self.held_x != self.x:
-                print("INDEX OF EDIT: " + str(self.index_of_edit))
-                self.splines[self.spline_edit][self.index_of_edit] = [
-                    pxToM(self.x), pxToM(self.y)
-                ]
+                self.points.setSplines(self.spline_edit, self.index_of_edit,
+                                       pxToM(self.x), pxToM(self.y))
 
-                if not self.spline_edit == len(self.splines) - 1:
-                    spline_edit = self.spline_edit + 1
-                    f = self.splines[self.spline_edit][5]
-                    e = self.splines[self.spline_edit][4]
-                    d = self.splines[self.spline_edit][3]
-                    self.splines[spline_edit][0] = f
-                    self.splines[spline_edit][1] = f * 2 + e * -1
-                    self.splines[spline_edit][2] = d + f * 4 + e * -4
-
-                if not self.spline_edit == 0:
-                    spline_edit = self.spline_edit - 1
-                    a = self.splines[self.spline_edit][0]
-                    b = self.splines[self.spline_edit][1]
-                    c = self.splines[self.spline_edit][2]
-                    self.splines[spline_edit][5] = a
-                    self.splines[spline_edit][4] = a * 2 + b * -1
-                    self.splines[spline_edit][3] = c + a * 4 + b * -4
+                self.spline_edit = self.points.splineExtrapolate(
+                    self.spline_edit)
 
                 self.index_of_edit = -1
                 self.spline_edit = -1
             else:
-                print("mode == 2")
                 # Get clicked point
                 # Find nearest
                 # Move nearest to clicked
                 cur_p = [pxToM(self.x), pxToM(self.y)]
-                print("CUR_P: " + str(self.x) + " " + str(self.y))
                 # Get the distance between each for x and y
                 # Save the index of the point closest
                 nearest = 1  # Max distance away a the selected point can be in meters
                 index_of_closest = 0
-                for index_splines, points in enumerate(self.splines):
+                for index_splines, points in enumerate(self.points.getSplines()):
                     for index_points, val in enumerate(points):
-                        # pythagorean
                         distance = np.sqrt((cur_p[0] - val[0])**2 +
                                            (cur_p[1] - val[1])**2)
                         if distance < nearest:
@@ -591,10 +391,6 @@
                             self.index_of_edit = index_of_closest
                             self.spline_edit = index_splines
                             self.held_x = self.x
-        elif self.mode == Mode.kConstraint:
-            print("RAN")
-            self.set_index_to_nearest_spline_point()
-            print("FINISHED")
 
     def do_button_press(self, event):
         print("button press activated")
@@ -612,13 +408,23 @@
         print("Method_connect ran")
         self.connect(event, handler)
 
+    def mouse_move(self, event):
+        #Changes event.x and event.y to be relative to the center.
+        x = event.x - self.drawing_area.window_shape[0] / 2
+        y = self.drawing_area.window_shape[1] / 2 - event.y
+        scale = self.drawing_area.get_current_scale()
+        event.x = x / scale + self.drawing_area.center[0]
+        event.y = y / scale + self.drawing_area.center[1]
+        self.drawing_area.mouse_move(event)
+        self.queue_draw()
+
     def button_press(self, event):
         print("button press activated")
         o_x = event.x
         o_y = event.y
         x = event.x - self.drawing_area.window_shape[0] / 2
         y = self.drawing_area.window_shape[1] / 2 - event.y
-        scale = self.drawing_area.get_current_scale()
+        scale = 2 * self.drawing_area.get_current_scale()
         event.x = x / scale + self.drawing_area.center[0]
         event.y = y / scale + self.drawing_area.center[1]
         self.drawing_area.do_button_press(event)
@@ -627,21 +433,17 @@
 
     def key_press(self, event):
         print("key press activated")
-        self.drawing_area.do_key_press(event)
+        self.drawing_area.do_key_press(event, self.file_name_box.get_text())
         self.queue_draw()
 
     def configure(self, event):
         print("configure activated")
         self.drawing_area.window_shape = (event.width, event.height)
 
-    def on_submit_click(self, widget):
-        self.drawing_area.inConstraint = int(self.constraint_box.get_text())
-        self.drawing_area.inValue = int(self.value_box.get_text())
-
     def __init__(self):
         Gtk.Window.__init__(self)
 
-        self.set_default_size(1366, 738)
+        self.set_default_size(1.5 * SCREEN_SIZE, SCREEN_SIZE)
 
         flowBox = Gtk.FlowBox()
         flowBox.set_valign(Gtk.Align.START)
@@ -663,41 +465,25 @@
                                  | Gdk.EventMask.SCROLL_MASK
                                  | Gdk.EventMask.KEY_PRESS_MASK)
 
+        #add the graph box
         self.drawing_area = GTK_Widget()
         self.eventBox.add(self.drawing_area)
 
         self.method_connect("key-release-event", self.key_press)
         self.method_connect("button-release-event", self.button_press)
         self.method_connect("configure-event", self.configure)
+        self.method_connect("motion_notify_event", self.mouse_move)
 
-        # Constraint Boxes
+        self.file_name_box = Gtk.Entry()
+        self.file_name_box.set_size_request(200, 40)
 
-        self.start_box = Gtk.Entry()
-        self.start_box.set_size_request(100, 20)
+        self.file_name_box.set_text("File")
+        self.file_name_box.set_editable(True)
 
-        self.constraint_box = Gtk.Entry()
-        self.constraint_box.set_size_request(100, 20)
-
-        self.constraint_box.set_text("Constraint")
-        self.constraint_box.set_editable(True)
-
-        container.put(self.constraint_box, 700, 0)
-
-        self.value_box = Gtk.Entry()
-        self.value_box.set_size_request(100, 20)
-
-        self.value_box.set_text("Value")
-        self.value_box.set_editable(True)
-
-        container.put(self.value_box, 700, 40)
-
-        self.submit_button = Gtk.Button("Submit")
-        self.submit_button.connect('clicked', self.on_submit_click)
-
-        container.put(self.submit_button, 880, 0)
+        container.put(self.file_name_box, 0, 0)
 
         self.show_all()
 
 
 window = GridWindow()
-basic_window.RunApp()
+RunApp()
diff --git a/frc971/control_loops/python/points.py b/frc971/control_loops/python/points.py
new file mode 100644
index 0000000..8f94e3f
--- /dev/null
+++ b/frc971/control_loops/python/points.py
@@ -0,0 +1,110 @@
+from constants import *
+import numpy as np
+from libspline import Spline, DistanceSpline, Trajectory
+
+
+class Points():
+    def __init__(self):
+        self.points = []  # Holds all points not yet in spline
+        self.libsplines = []  # Formatted for libspline library usage
+        self.splines = []  # Formatted for drawing
+
+    def getPoints(self):
+        return self.points
+
+    def resetPoints(self):
+        self.points = []
+
+    def getLibsplines(self):
+        return self.libsplines
+
+    def splineExtrapolate(self, o_spline_edit):
+        spline_edit = o_spline_edit
+        if not spline_edit == len(self.splines) - 1:
+            spline_edit = spline_edit + 1
+            f = self.splines[spline_edit][5]
+            e = self.splines[spline_edit][4]
+            d = self.splines[spline_edit][3]
+            self.splines[spline_edit][0] = f
+            self.splines[spline_edit][1] = f * 2 + e * -1
+            self.splines[spline_edit][2] = d + f * 4 + e * -4
+
+        if not spline_edit == 0:
+            spline_edit = spline_edit - 1
+            a = self.splines[spline_edit][0]
+            b = self.splines[spline_edit][1]
+            c = self.splines[spline_edit][2]
+            self.splines[spline_edit][5] = a
+            self.splines[spline_edit][4] = a * 2 + b * -1
+            self.splines[spline_edit][3] = c + a * 4 + b * -4
+
+        return spline_edit
+
+    def updates_for_mouse_move(self, index_of_edit, spline_edit, x, y, difs):
+        if index_of_edit > -1:
+            self.splines[spline_edit][index_of_edit] = [pxToM(x), pxToM(y)]
+
+            if index_of_edit == 5:
+                self.splines[spline_edit][
+                    index_of_edit -
+                    2] = self.splines[spline_edit][index_of_edit - 2] + difs
+                self.splines[spline_edit][
+                    index_of_edit -
+                    1] = self.splines[spline_edit][index_of_edit - 1] + difs
+
+            if index_of_edit == 0:
+                self.splines[spline_edit][
+                    index_of_edit +
+                    2] = self.splines[spline_edit][index_of_edit + 2] + difs
+                self.splines[spline_edit][
+                    index_of_edit +
+                    1] = self.splines[spline_edit][index_of_edit + 1] + difs
+
+            if index_of_edit == 4:
+                self.splines[spline_edit][
+                    index_of_edit -
+                    1] = self.splines[spline_edit][index_of_edit - 1] + difs
+
+            if index_of_edit == 1:
+                self.splines[spline_edit][
+                    index_of_edit +
+                    1] = self.splines[spline_edit][index_of_edit + 1] + difs
+
+            return self.splineExtrapolate(spline_edit)
+
+    def update_lib_spline(self):
+        self.libsplines = []
+        array = np.zeros(shape=(6, 2), dtype=float)
+        for points in self.splines:
+            for j, point in enumerate(points):
+                array[j, 0] = point[0]
+                array[j, 1] = point[1]
+            spline = Spline(np.ascontiguousarray(np.transpose(array)))
+            self.libsplines.append(spline)
+
+    def getSplines(self):
+        return self.splines
+
+    def resetSplines(self):
+        self.splines = []
+
+    def setUpSplines(self, newSplines):
+        self.splines = newSplines
+
+    def setSplines(self, spline_edit, index_of_edit, x, y):
+        self.splines[spline_edit][index_of_edit] = [x, y]
+
+    def add_point(self, x, y):
+        if (len(self.points) < 6):
+            self.points.append([pxToM(x), pxToM(y)])
+        if (len(self.points) == 6):
+            self.splines.append(np.array(self.points))
+            self.points = []
+            self.update_lib_spline()
+            return True
+
+    def extrapolate(self, point1, point2,
+                    point3):  # where point3 is 3rd to last point
+        self.points.append(point1)
+        self.points.append(point1 * 2 - point2)
+        self.points.append(point3 + point1 * 4 - point2 * 4)
diff --git a/frc971/control_loops/python/spline.py b/frc971/control_loops/python/spline.py
index e69d874..b428c9a 100644
--- a/frc971/control_loops/python/spline.py
+++ b/frc971/control_loops/python/spline.py
@@ -15,7 +15,6 @@
 from frc971.control_loops.python import drivetrain
 from frc971.control_loops.python import controls
 import y2016.control_loops.python.drivetrain
-
 """This file is my playground for implementing spline following.
 
 All splines here are cubic bezier splines.  See
@@ -73,8 +72,9 @@
     if numpy.isscalar(alpha):
         alpha = [alpha]
     dalpha_matrix = [[
-        -3.0 * (1.0 - a)**2.0, 3.0 * (1.0 - a)**2.0 + -2.0 * 3.0 *
-        (1.0 - a) * a, -3.0 * a**2.0 + 2.0 * 3.0 * (1.0 - a) * a, 3.0 * a**2.0
+        -3.0 * (1.0 - a)**2.0,
+        3.0 * (1.0 - a)**2.0 + -2.0 * 3.0 * (1.0 - a) * a,
+        -3.0 * a**2.0 + 2.0 * 3.0 * (1.0 - a) * a, 3.0 * a**2.0
     ] for a in alpha]
 
     return control_points * numpy.matrix(dalpha_matrix).T
@@ -97,8 +97,7 @@
     ddalpha_matrix = [[
         2.0 * 3.0 * (1.0 - a),
         -2.0 * 3.0 * (1.0 - a) + -2.0 * 3.0 * (1.0 - a) + 2.0 * 3.0 * a,
-        -2.0 * 3.0 * a + 2.0 * 3.0 * (1.0 - a) - 2.0 * 3.0 * a,
-        2.0 * 3.0 * a
+        -2.0 * 3.0 * a + 2.0 * 3.0 * (1.0 - a) - 2.0 * 3.0 * a, 2.0 * 3.0 * a
     ] for a in alpha]
 
     return control_points * numpy.matrix(ddalpha_matrix).T
@@ -119,10 +118,8 @@
     if numpy.isscalar(alpha):
         alpha = [alpha]
     ddalpha_matrix = [[
-        -2.0 * 3.0,
-        2.0 * 3.0 + 2.0 * 3.0 + 2.0 * 3.0,
-        -2.0 * 3.0 - 2.0 * 3.0 - 2.0 * 3.0,
-        2.0 * 3.0
+        -2.0 * 3.0, 2.0 * 3.0 + 2.0 * 3.0 + 2.0 * 3.0,
+        -2.0 * 3.0 - 2.0 * 3.0 - 2.0 * 3.0, 2.0 * 3.0
     ] for a in alpha]
 
     return control_points * numpy.matrix(ddalpha_matrix).T
@@ -143,7 +140,8 @@
         dspline_points = dspline(alpha, control_points)
 
     return numpy.arctan2(
-        numpy.array(dspline_points)[1, :], numpy.array(dspline_points)[0, :])
+        numpy.array(dspline_points)[1, :],
+        numpy.array(dspline_points)[0, :])
 
 
 def dspline_theta(alpha,
@@ -219,8 +217,8 @@
     dddy = numpy.array(dddspline_points)[1, :]
 
     return -1.0 / ((dx**2.0 + dy**2.0)**2.0) * (dx * ddy - dy * ddx) * 2.0 * (
-        dy * ddy + dx * ddx) + 1.0 / (dx**2.0 + dy**2.0) * (dx * dddy - dy *
-                                                            dddx)
+        dy * ddy + dx * ddx) + 1.0 / (dx**2.0 + dy**2.0) * (dx * dddy -
+                                                            dy * dddx)
 
 
 class Path(object):
@@ -230,7 +228,8 @@
         self._control_points = control_points
 
         def spline_velocity(alpha):
-            return numpy.linalg.norm(dspline(alpha, self._control_points), axis=0)
+            return numpy.linalg.norm(dspline(alpha, self._control_points),
+                                     axis=0)
 
         self._point_distances = [0.0]
         num_alpha = 100
@@ -240,8 +239,8 @@
         # might think.
         for alpha in numpy.linspace(0.0, 1.0, num_alpha)[1:]:
             self._point_distances.append(
-                scipy.integrate.fixed_quad(spline_velocity, alpha, alpha + 1.0
-                                           / (num_alpha - 1.0))[0] +
+                scipy.integrate.fixed_quad(spline_velocity, alpha, alpha +
+                                           1.0 / (num_alpha - 1.0))[0] +
                 self._point_distances[-1])
 
     def distance_to_alpha(self, distance):
@@ -256,7 +255,8 @@
         if numpy.isscalar(distance):
             return numpy.array([self._distance_to_alpha_scalar(distance)])
         else:
-            return numpy.array([self._distance_to_alpha_scalar(d) for d in distance])
+            return numpy.array(
+                [self._distance_to_alpha_scalar(d) for d in distance])
 
     def _distance_to_alpha_scalar(self, distance):
         """Helper to compute alpha for a distance for a single scalar."""
@@ -264,15 +264,17 @@
             return 0.0
         elif distance >= self.length():
             return 1.0
-        after_index = numpy.searchsorted(
-            self._point_distances, distance, side='right')
+        after_index = numpy.searchsorted(self._point_distances,
+                                         distance,
+                                         side='right')
         before_index = after_index - 1
 
         # Linearly interpolate alpha from our (sorted) distance table.
         return (distance - self._point_distances[before_index]) / (
             self._point_distances[after_index] -
-            self._point_distances[before_index]) * (1.0 / (
-                len(self._point_distances) - 1.0)) + float(before_index) / (
+            self._point_distances[before_index]) * (
+                1.0 /
+                (len(self._point_distances) - 1.0)) + float(before_index) / (
                     len(self._point_distances) - 1.0)
 
     def length(self):
@@ -287,8 +289,8 @@
     # TODO(austin): need a better name...
     def dxy(self, distance):
         """Returns the xy velocity as a function of distance."""
-        dspline_point = dspline(
-            self.distance_to_alpha(distance), self._control_points)
+        dspline_point = dspline(self.distance_to_alpha(distance),
+                                self._control_points)
         return dspline_point / numpy.linalg.norm(dspline_point, axis=0)
 
     # TODO(austin): need a better name...
@@ -298,8 +300,7 @@
         dspline_points = dspline(alpha, self._control_points)
         ddspline_points = ddspline(alpha, self._control_points)
 
-        norm = numpy.linalg.norm(
-            dspline_points, axis=0)**2.0
+        norm = numpy.linalg.norm(dspline_points, axis=0)**2.0
 
         return ddspline_points / norm - numpy.multiply(
             dspline_points, (numpy.array(dspline_points)[0, :] *
@@ -309,10 +310,9 @@
 
     def theta(self, distance, dspline_points=None):
         """Returns the heading as a function of distance."""
-        return spline_theta(
-            self.distance_to_alpha(distance),
-            self._control_points,
-            dspline_points=dspline_points)
+        return spline_theta(self.distance_to_alpha(distance),
+                            self._control_points,
+                            dspline_points=dspline_points)
 
     def dtheta(self, distance, dspline_points=None, ddspline_points=None):
         """Returns the angular velocity as a function of distance."""
@@ -327,9 +327,14 @@
 
         return dtheta_points / numpy.linalg.norm(dspline_points, axis=0)
 
-    def dtheta_dt(self, distance, velocity, dspline_points=None, ddspline_points=None):
+    def dtheta_dt(self,
+                  distance,
+                  velocity,
+                  dspline_points=None,
+                  ddspline_points=None):
         """Returns the angular velocity as a function of time."""
-        return self.dtheta(distance, dspline_points, ddspline_points) * velocity
+        return self.dtheta(distance, dspline_points,
+                           ddspline_points) * velocity
 
     def ddtheta(self,
                 distance,
@@ -400,8 +405,8 @@
             return 0.0
         return (f(t, y) - a0) / y
 
-    return (RungeKutta(integrablef, v, x, dx) - v
-            ) + numpy.sqrt(2.0 * a0 * dx + v * v)
+    return (RungeKutta(integrablef, v, x, dx) - v) + numpy.sqrt(2.0 * a0 * dx +
+                                                                v * v)
 
 
 class Trajectory(object):
@@ -409,8 +414,8 @@
                  distance_count):
         self._path = path
         self._drivetrain = drivetrain
-        self.distances = numpy.linspace(0.0,
-                                        self._path.length(), distance_count)
+        self.distances = numpy.linspace(0.0, self._path.length(),
+                                        distance_count)
         self._longitudal_accel = longitudal_accel
         self._lateral_accel = lateral_accel
 
@@ -460,9 +465,10 @@
         C = K3 * v * v + K4 * v
         # Note: K345 are not quite constant over the step, but we are going
         # to assume they are for now.
-        accelerations = [(12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) /
-                         K5[1, 0], (-12.0 - C[0, 0]) / K5[0, 0],
-                         (-12.0 - C[1, 0]) / K5[1, 0]]
+        accelerations = [
+            (12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) / K5[1, 0],
+            (-12.0 - C[0, 0]) / K5[0, 0], (-12.0 - C[1, 0]) / K5[1, 0]
+        ]
         maxa = -float('inf')
         for a in accelerations:
             U = K5 * a + K3 * v * v + K4 * v
@@ -473,8 +479,8 @@
         # Constrain the longitudinal acceleration to keep in a pseudo friction
         # circle.  This will make it so we don't floor it while in a turn and
         # cause extra wheel slip.
-        long_accel = numpy.sqrt(1.0 - (lateral_accel / self._lateral_accel)**
-                                2.0) * self._longitudal_accel
+        long_accel = numpy.sqrt(1.0 - (
+            lateral_accel / self._lateral_accel)**2.0) * self._longitudal_accel
         return min(long_accel, maxa)
 
     def forward_pass(self, plan):
@@ -503,9 +509,10 @@
         C = K3 * v * v + K4 * v
         # Note: K345 are not quite constant over the step, but we are going
         # to assume they are for now.
-        accelerations = [(12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) /
-                         K5[1, 0], (-12.0 - C[0, 0]) / K5[0, 0],
-                         (-12.0 - C[1, 0]) / K5[1, 0]]
+        accelerations = [
+            (12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) / K5[1, 0],
+            (-12.0 - C[0, 0]) / K5[0, 0], (-12.0 - C[1, 0]) / K5[1, 0]
+        ]
         mina = float('inf')
         for a in accelerations:
             U = K5 * a + K3 * v * v + K4 * v
@@ -516,8 +523,8 @@
         # Constrain the longitudinal acceleration to keep in a pseudo friction
         # circle.  This will make it so we don't floor it while in a turn and
         # cause extra wheel slip.
-        long_accel = -numpy.sqrt(1.0 - (lateral_accel / self._lateral_accel)**
-                                 2.0) * self._longitudal_accel
+        long_accel = -numpy.sqrt(1.0 - (
+            lateral_accel / self._lateral_accel)**2.0) * self._longitudal_accel
         return max(long_accel, mina)
 
     def backward_pass(self, plan):
@@ -546,8 +553,9 @@
             if distance > self.distances[-1]:
                 distance = self.distances[-1]
         else:
-            after_index = numpy.searchsorted(
-                self.distances, distance, side='right')
+            after_index = numpy.searchsorted(self.distances,
+                                             distance,
+                                             side='right')
             before_index = after_index - 1
 
         vforwards = integrate_accel_for_distance(
@@ -586,14 +594,15 @@
         return U
 
     def goal_state(self, distance, velocity):
-        width = (
-            self._drivetrain.robot_radius_l + self._drivetrain.robot_radius_r)
+        width = (self._drivetrain.robot_radius_l +
+                 self._drivetrain.robot_radius_r)
         goal = numpy.matrix(numpy.zeros((5, 1)))
 
         goal[0:2, :] = self._path.xy(distance)
         goal[2, :] = self._path.theta(distance)
 
-        Ter = numpy.linalg.inv(numpy.matrix([[0.5, 0.5], [-1.0 / width, 1.0 / width]]))
+        Ter = numpy.linalg.inv(
+            numpy.matrix([[0.5, 0.5], [-1.0 / width, 1.0 / width]]))
         goal[3:5, :] = Ter * numpy.matrix(
             [[velocity], [self._path.dtheta_dt(distance, velocity)]])
         return goal
@@ -618,25 +627,23 @@
 
     # Compute theta and the two derivatives
     theta = spline_theta(alphas, control_points, dspline_points=dspline_points)
-    dtheta = dspline_theta(
-        alphas, control_points, dspline_points=dspline_points)
-    ddtheta = ddspline_theta(
-        alphas,
-        control_points,
-        dspline_points=dspline_points,
-        dddspline_points=dddspline_points)
+    dtheta = dspline_theta(alphas,
+                           control_points,
+                           dspline_points=dspline_points)
+    ddtheta = ddspline_theta(alphas,
+                             control_points,
+                             dspline_points=dspline_points,
+                             dddspline_points=dddspline_points)
 
     # Plot the control points and the spline.
     pylab.figure()
-    pylab.plot(
-        numpy.array(control_points)[0, :],
-        numpy.array(control_points)[1, :],
-        '-o',
-        label='control')
-    pylab.plot(
-        numpy.array(spline_points)[0, :],
-        numpy.array(spline_points)[1, :],
-        label='spline')
+    pylab.plot(numpy.array(control_points)[0, :],
+               numpy.array(control_points)[1, :],
+               '-o',
+               label='control')
+    pylab.plot(numpy.array(spline_points)[0, :],
+               numpy.array(spline_points)[1, :],
+               label='spline')
     pylab.legend()
 
     # For grins, confirm that the double integral of the acceleration (with
@@ -657,9 +664,9 @@
     # Integrate up the spline velocity and heading to confirm that given a
     # velocity (as a function of the spline parameter) and angle, we will move
     # from the starting point to the ending point.
-    thetaint_plot = numpy.zeros((len(alphas),))
+    thetaint_plot = numpy.zeros((len(alphas), ))
     thetaint = theta[0]
-    dthetaint_plot = numpy.zeros((len(alphas),))
+    dthetaint_plot = numpy.zeros((len(alphas), ))
     dthetaint = dtheta[0]
     thetaint_plot[0] = thetaint
     dthetaint_plot[0] = dthetaint
@@ -670,15 +677,14 @@
     for i in range(len(alphas) - 1):
         dalpha = alphas[i + 1] - alphas[i]
         txint += dalpha * numpy.linalg.norm(
-            dspline_points[:, i]) * numpy.matrix(
-                [[numpy.cos(theta[i])], [numpy.sin(theta[i])]])
+            dspline_points[:, i]) * numpy.matrix([[numpy.cos(theta[i])],
+                                                  [numpy.sin(theta[i])]])
         txint_plot[:, i + 1] = txint
         thetaint += dalpha * dtheta[i]
         dthetaint += dalpha * ddtheta[i]
         thetaint_plot[i + 1] = thetaint
         dthetaint_plot[i + 1] = dthetaint
 
-
     # Now plot x, dx/dalpha, ddx/ddalpha, dddx/dddalpha, and integrals thereof
     # to perform consistency checks.
     pylab.figure()
@@ -709,11 +715,9 @@
     pylab.plot(alphas, ddtheta, label='ddtheta')
     pylab.plot(alphas, thetaint_plot, label='thetai')
     pylab.plot(alphas, dthetaint_plot, label='dthetai')
-    pylab.plot(
-        alphas,
-        numpy.linalg.norm(
-            numpy.array(dspline_points), axis=0),
-        label='velocity')
+    pylab.plot(alphas,
+               numpy.linalg.norm(numpy.array(dspline_points), axis=0),
+               label='velocity')
 
     # Now, repeat as a function of path length as opposed to alpha
     path = Path(control_points)
@@ -780,12 +784,11 @@
     longitudal_accel = 3.0
     lateral_accel = 2.0
 
-    trajectory = Trajectory(
-        path,
-        drivetrain=velocity_drivetrain,
-        longitudal_accel=longitudal_accel,
-        lateral_accel=lateral_accel,
-        distance_count=500)
+    trajectory = Trajectory(path,
+                            drivetrain=velocity_drivetrain,
+                            longitudal_accel=longitudal_accel,
+                            lateral_accel=lateral_accel,
+                            distance_count=500)
 
     vmax = numpy.inf
     vmax = 10.0
@@ -827,14 +830,16 @@
 
     while spline_state[0, 0] < path.length():
         t += dt
+
         def along_spline_diffeq(t, x):
             _, v, a = trajectory.ff_accel(backward_accel_plan, x[0, 0])
             return numpy.matrix([[x[1, 0]], [a]])
 
-        spline_state = RungeKutta(along_spline_diffeq,
-                                  spline_state.copy(), t, dt)
+        spline_state = RungeKutta(along_spline_diffeq, spline_state.copy(), t,
+                                  dt)
 
-        d, v, a = trajectory.ff_accel(backward_accel_plan, length_plan_x[-1][0, 0])
+        d, v, a = trajectory.ff_accel(backward_accel_plan,
+                                      length_plan_x[-1][0, 0])
 
         length_plan_v.append(v)
         length_plan_a.append(a)
@@ -877,9 +882,10 @@
              [path.dtheta_dt(xva_plan[0, i], xva_plan[1, i])[0]]])
         vel_lr = numpy.linalg.inv(Ter) * vel_omega_r
 
-        state_plan[:, i] = numpy.matrix(
-            [[x_r], [y_r], [theta_r], [vel_lr[0, 0]], [vel_lr[1, 0]]])
-        u_plan[:, i] = trajectory.ff_voltage(backward_accel_plan, xva_plan[0, i])
+        state_plan[:, i] = numpy.matrix([[x_r], [y_r], [theta_r],
+                                         [vel_lr[0, 0]], [vel_lr[1, 0]]])
+        u_plan[:, i] = trajectory.ff_voltage(backward_accel_plan, xva_plan[0,
+                                                                           i])
 
     Q = numpy.matrix(
         numpy.diag([
@@ -906,16 +912,17 @@
             linear_velocity = -kMinVelocity
 
         width = (velocity_drivetrain.robot_radius_l +
-                velocity_drivetrain.robot_radius_r)
-        A_linearized_continuous = numpy.matrix([[
-            0.0, 0.0, -sintheta * linear_velocity, 0.5 * costheta, 0.5 *
-            costheta
-        ], [
-            0.0, 0.0, costheta * linear_velocity, 0.5 * sintheta, 0.5 *
-            sintheta
-        ], [0.0, 0.0, 0.0, -1.0 / width, 1.0 / width],
-                                                [0.0, 0.0, 0.0, 0.0, 0.0],
-                                                [0.0, 0.0, 0.0, 0.0, 0.0]])
+                 velocity_drivetrain.robot_radius_r)
+        A_linearized_continuous = numpy.matrix(
+            [[
+                0.0, 0.0, -sintheta * linear_velocity, 0.5 * costheta,
+                0.5 * costheta
+            ],
+             [
+                 0.0, 0.0, costheta * linear_velocity, 0.5 * sintheta,
+                 0.5 * sintheta
+             ], [0.0, 0.0, 0.0, -1.0 / width, 1.0 / width],
+             [0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0]])
         A_linearized_continuous[3:5, 3:5] = velocity_drivetrain.A_continuous
         B_linearized_continuous = numpy.matrix(numpy.zeros((5, 2)))
         B_linearized_continuous[3:5, :] = velocity_drivetrain.B_continuous
@@ -942,15 +949,15 @@
                 velocity_drivetrain.robot_radius_r)
             accel = (velocity_drivetrain.A_continuous * velocity +
                      velocity_drivetrain.B_continuous * U)
-            return numpy.matrix(
-                [[numpy.cos(theta) * linear_velocity],
-                 [numpy.sin(theta) * linear_velocity], [angular_velocity],
-                 [accel[0, 0]], [accel[1, 0]]])
+            return numpy.matrix([[numpy.cos(theta) * linear_velocity],
+                                 [numpy.sin(theta) * linear_velocity],
+                                 [angular_velocity], [accel[0, 0]],
+                                 [accel[1, 0]]])
 
         full_us[:, i] = U
 
-        state = RungeKutta(lambda t, x: spline_diffeq(U, t, x),
-                                state, i * dt, dt)
+        state = RungeKutta(lambda t, x: spline_diffeq(U, t, x), state, i * dt,
+                           dt)
 
     pylab.figure()
     pylab.plot(length_plan_t, numpy.array(xva_plan)[0, :], label='x')
@@ -963,17 +970,14 @@
     pylab.legend()
 
     pylab.figure()
-    pylab.plot(
-        numpy.array(states)[0, :],
-        numpy.array(states)[1, :],
-        label='robot')
-    pylab.plot(
-        numpy.array(spline_points)[0, :],
-        numpy.array(spline_points)[1, :],
-        label='spline')
+    pylab.plot(numpy.array(states)[0, :],
+               numpy.array(states)[1, :],
+               label='robot')
+    pylab.plot(numpy.array(spline_points)[0, :],
+               numpy.array(spline_points)[1, :],
+               label='spline')
     pylab.legend()
 
-
     def a(_, x):
         return 2.0
         return 2.0 + 0.0001 * x
diff --git a/frc971/control_loops/python/spline_drawing.py b/frc971/control_loops/python/spline_drawing.py
new file mode 100644
index 0000000..4e8b04b
--- /dev/null
+++ b/frc971/control_loops/python/spline_drawing.py
@@ -0,0 +1,292 @@
+from color import Color, palette
+import cairo
+
+import numpy as np
+from basic_window import OverrideMatrix, identity, quit_main_loop, set_color
+WIDTH_OF_FIELD_IN_METERS = 8.258302
+PIXELS_ON_SCREEN = 800
+
+X_TRANSLATE = 0
+Y_TRANSLATE = 0
+
+
+def pxToM(p):
+    return p * WIDTH_OF_FIELD_IN_METERS / PIXELS_ON_SCREEN
+
+
+def mToPx(m):
+    return (m * PIXELS_ON_SCREEN / WIDTH_OF_FIELD_IN_METERS)
+
+
+def px(cr):
+    return OverrideMatrix(cr, identity)
+
+
+def draw_px_cross(cr, x, y, length_px, color=palette["RED"]):
+    """Draws a cross with fixed dimensions in pixel space."""
+    set_color(cr, color)
+    cr.move_to(x, y - length_px)
+    cr.line_to(x, y + length_px)
+    cr.stroke()
+
+    cr.move_to(x - length_px, y)
+    cr.line_to(x + length_px, y)
+    cr.stroke()
+    set_color(cr, palette["LIGHT_GREY"])
+
+
+def draw_px_x(cr, x, y, length_px1, color=palette["BLACK"]):
+    """Draws a x with fixed dimensions in pixel space."""
+    length_px = length_px1 / np.sqrt(2)
+    set_color(cr, color)
+    cr.move_to(x - length_px, y - length_px)
+    cr.line_to(x + length_px, y + length_px)
+    cr.stroke()
+
+    cr.move_to(x - length_px, y + length_px)
+    cr.line_to(x + length_px, y - length_px)
+    cr.stroke()
+    set_color(cr, palette["LIGHT_GREY"])
+
+
+def display_text(cr, text, widtha, heighta, widthb, heightb):
+    cr.scale(widtha, -heighta)
+    cr.show_text(text)
+    cr.scale(widthb, -heightb)
+
+
+def redraw(needs_redraw, window):
+    print("Redrew")
+    if not needs_redraw:
+        window.queue_draw()
+
+
+def draw_HAB(cr):
+    print("WENT IN")
+    # BASE Constants
+    X_BASE = 0 + X_TRANSLATE  #(2.41568)
+    Y_BASE = 0 + Y_TRANSLATE  #mToPx(4.129151)
+    BACKWALL_X = X_TRANSLATE
+    LOADING_Y = mToPx(4.129151) - mToPx(0.696976)
+    # HAB Levels 2 and 3 called in variables backhab
+    # draw loading stations
+    cr.move_to(0, LOADING_Y)
+    cr.line_to(mToPx(0.6), LOADING_Y)
+    cr.move_to(0, -1.0 * LOADING_Y)
+    cr.line_to(mToPx(0.6), -1.0 * LOADING_Y)
+
+    BACKWALL_X = X_TRANSLATE
+
+    # HAB Levels 2 and 3 called in variables backhab
+
+    WIDTH_BACKHAB = mToPx(1.2192)
+
+    Y_TOP_BACKHAB_BOX = Y_BASE + mToPx(0.6096)
+    BACKHAB_LV2_LENGTH = mToPx(1.016)
+
+    BACKHAB_LV3_LENGTH = mToPx(1.2192)
+    Y_LV3_BOX = Y_TOP_BACKHAB_BOX - BACKHAB_LV3_LENGTH
+
+    Y_BOTTOM_BACKHAB_BOX = Y_LV3_BOX - BACKHAB_LV2_LENGTH
+
+    # HAB LEVEL 1
+    X_LV1_BOX = BACKWALL_X + WIDTH_BACKHAB
+
+    WIDTH_LV1_BOX = mToPx(0.90805)
+    LENGTH_LV1_BOX = mToPx(1.6256)
+
+    Y_BOTTOM_LV1_BOX = Y_BASE - LENGTH_LV1_BOX
+
+    # Ramp off Level 1
+    X_RAMP = X_LV1_BOX
+
+    Y_TOP_RAMP = Y_BASE + LENGTH_LV1_BOX
+    WIDTH_TOP_RAMP = mToPx(1.20015)
+    LENGTH_TOP_RAMP = Y_BASE + mToPx(0.28306)
+
+    X_MIDDLE_RAMP = X_RAMP + WIDTH_LV1_BOX
+    Y_MIDDLE_RAMP = Y_BOTTOM_LV1_BOX
+    LENGTH_MIDDLE_RAMP = 2 * LENGTH_LV1_BOX
+    WIDTH_MIDDLE_RAMP = WIDTH_TOP_RAMP - WIDTH_LV1_BOX
+
+    Y_BOTTOM_RAMP = Y_BASE - LENGTH_LV1_BOX - LENGTH_TOP_RAMP
+
+    # Side Bars to Hold in balls
+    X_BARS = BACKWALL_X
+    WIDTH_BARS = WIDTH_BACKHAB
+    LENGTH_BARS = mToPx(0.574675)
+
+    Y_TOP_BAR = Y_TOP_BACKHAB_BOX + BACKHAB_LV2_LENGTH
+
+    Y_BOTTOM_BAR = Y_BOTTOM_BACKHAB_BOX - LENGTH_BARS
+
+    set_color(cr, palette["BLACK"])
+    cr.rectangle(BACKWALL_X, Y_TOP_BACKHAB_BOX, WIDTH_BACKHAB,
+                 BACKHAB_LV2_LENGTH)
+    cr.rectangle(BACKWALL_X, Y_LV3_BOX, WIDTH_BACKHAB, BACKHAB_LV3_LENGTH)
+    cr.rectangle(BACKWALL_X, Y_BOTTOM_BACKHAB_BOX, WIDTH_BACKHAB,
+                 BACKHAB_LV2_LENGTH)
+    cr.rectangle(X_LV1_BOX, Y_BASE, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
+    cr.rectangle(X_LV1_BOX, Y_BOTTOM_LV1_BOX, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
+    cr.rectangle(X_RAMP, Y_TOP_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
+    cr.rectangle(X_MIDDLE_RAMP, Y_MIDDLE_RAMP, WIDTH_MIDDLE_RAMP,
+                 LENGTH_MIDDLE_RAMP)
+    cr.rectangle(X_RAMP, Y_BOTTOM_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
+    cr.rectangle(X_BARS, Y_TOP_BAR, WIDTH_BARS, LENGTH_BARS)
+    cr.rectangle(X_BARS, Y_BOTTOM_BAR, WIDTH_BARS, LENGTH_BARS)
+    cr.stroke()
+
+    cr.set_line_join(cairo.LINE_JOIN_ROUND)
+
+    cr.stroke()
+
+    #draw 0, 0
+    set_color(cr, palette["BLACK"])
+    cr.move_to(X_TRANSLATE, Y_TRANSLATE)
+    cr.line_to(X_TRANSLATE, Y_TRANSLATE + mToPx(8.2296 / 2.0))
+    cr.move_to(X_TRANSLATE, Y_TRANSLATE)
+    cr.line_to(X_TRANSLATE, Y_TRANSLATE + mToPx(-8.2296 / 2.0))
+    cr.move_to(X_TRANSLATE, Y_TRANSLATE)
+    cr.line_to(X_TRANSLATE + mToPx(8.2296), Y_TRANSLATE)
+
+    cr.stroke()
+
+
+def draw_rockets(cr):
+    # BASE Constants
+    X_BASE = 0 + X_TRANSLATE + mToPx(2.41568)
+    Y_BASE = 0 + Y_TRANSLATE  #mToPx(4.129151)
+
+    near_side_rocket_center = [
+        X_BASE + mToPx((2.89973 + 3.15642) / 2.0), Y_BASE + mToPx(
+            (3.86305 + 3.39548) / 2.0)
+    ]
+    middle_rocket_center = [
+        X_BASE + mToPx((3.15642 + 3.6347) / 2.0), Y_BASE + mToPx(
+            (3.39548 + 3.392380) / 2.0)
+    ]
+    far_side_rocket_center = [
+        X_BASE + mToPx((3.63473 + 3.89984) / 2.0), Y_BASE + mToPx(
+            (3.39238 + 3.86305) / 2.0)
+    ]
+
+    cr.move_to(near_side_rocket_center[0], near_side_rocket_center[1])
+    cr.line_to(near_side_rocket_center[0] - 0.4 * mToPx(0.866),
+               near_side_rocket_center[1] - 0.4 * mToPx(0.5))
+
+    cr.move_to(middle_rocket_center[0], middle_rocket_center[1])
+    cr.line_to(middle_rocket_center[0], middle_rocket_center[1] - mToPx(0.4))
+
+    cr.move_to(far_side_rocket_center[0], far_side_rocket_center[1])
+    cr.line_to(far_side_rocket_center[0] + 0.4 * mToPx(0.866),
+               far_side_rocket_center[1] - 0.4 * mToPx(0.5))
+
+    print("FAR SIDE ROCKET")
+    #print(far_side_rocket_center)
+    near_side_rocket_center = [
+        X_BASE + mToPx((2.89973 + 3.15642) / 2.0), Y_BASE - mToPx(
+            (3.86305 + 3.39548) / 2.0)
+    ]
+    middle_rocket_center = [
+        X_BASE + mToPx((3.15642 + 3.6347) / 2.0), Y_BASE - mToPx(
+            (3.39548 + 3.392380) / 2.0)
+    ]
+    far_side_rocket_center = [
+        X_BASE + mToPx((3.63473 + 3.89984) / 2.0), Y_BASE - mToPx(
+            (3.39238 + 3.86305) / 2.0)
+    ]
+
+    cr.move_to(near_side_rocket_center[0], near_side_rocket_center[1])
+    cr.line_to(near_side_rocket_center[0] - 0.4 * mToPx(0.866),
+               near_side_rocket_center[1] + 0.4 * mToPx(0.5))
+
+    cr.move_to(middle_rocket_center[0], middle_rocket_center[1])
+    cr.line_to(middle_rocket_center[0], middle_rocket_center[1] + mToPx(0.4))
+
+    cr.move_to(far_side_rocket_center[0], far_side_rocket_center[1])
+    cr.line_to(far_side_rocket_center[0] + 0.4 * mToPx(0.866),
+               far_side_rocket_center[1] + 0.4 * mToPx(0.5))
+
+    X_BASE = 0 + X_TRANSLATE  #mToPx(2.41568)
+    Y_BASE = 0 + Y_TRANSLATE  #mToPx(4.129151)
+
+    # Leftmost Line
+    cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
+    cr.line_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
+
+    # Top Line
+    cr.move_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
+    cr.line_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
+
+    #Rightmost Line
+    cr.move_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
+    cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
+
+    #Back Line
+    cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
+    cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
+
+    # Leftmost Line
+    cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
+    cr.line_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
+
+    # Top Line
+    cr.move_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
+    cr.line_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
+
+    #Rightmost Line
+    cr.move_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
+    cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
+
+    #Back Line
+    cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
+    cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
+
+    cr.stroke()
+
+
+def draw_cargo_ship(cr):
+    # BASE Constants
+
+    X_BASE = X_TRANSLATE + mToPx(5.59435)
+    Y_BASE = 0 + Y_TRANSLATE  #mToPx(4.129151)
+
+    FRONT_PEG_DELTA_Y = mToPx(0.276352)
+    cr.move_to(X_BASE, Y_BASE + FRONT_PEG_DELTA_Y)
+    cr.line_to(X_BASE - mToPx(0.4), Y_BASE + FRONT_PEG_DELTA_Y)
+
+    cr.move_to(X_BASE, Y_BASE - FRONT_PEG_DELTA_Y)
+    cr.line_to(X_BASE - mToPx(0.4), Y_BASE - FRONT_PEG_DELTA_Y)
+
+    SIDE_PEG_Y = mToPx(1.41605 / 2.0)
+    SIDE_PEG_X = X_BASE + mToPx(1.148842)
+    SIDE_PEG_DX = mToPx(0.55245)
+
+    cr.move_to(SIDE_PEG_X, SIDE_PEG_Y)
+    cr.line_to(SIDE_PEG_X, SIDE_PEG_Y + mToPx(0.4))
+
+    cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y)
+    cr.line_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y + mToPx(0.4))
+
+    cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y)
+    cr.line_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y + mToPx(0.4))
+
+    cr.move_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y)
+    cr.line_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y - mToPx(0.4))
+
+    cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y)
+    cr.line_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(0.4))
+
+    cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y)
+    cr.line_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(0.4))
+
+    cr.rectangle(X_BASE, Y_BASE - mToPx(1.41605 / 2.0), mToPx(2.43205),
+                 mToPx(1.41605))
+
+    X_BASE = X_TRANSLATE + mToPx(2.41568)
+    Y_BASE = 0 + Y_TRANSLATE  #mToPx(4.129151)
+
+    cr.rectangle(X_BASE + mToPx(3.15912), Y_BASE - mToPx(0.72326),
+                 mToPx(2.43205), mToPx(1.41605))
+
+    cr.stroke()
diff --git a/frc971/control_loops/python/spline_graph.py b/frc971/control_loops/python/spline_graph.py
new file mode 100755
index 0000000..2fd7964
--- /dev/null
+++ b/frc971/control_loops/python/spline_graph.py
@@ -0,0 +1,100 @@
+#!/usr/bin/python3
+import gi
+from path_edit import *
+import numpy as np
+gi.require_version('Gtk', '3.0')
+from gi.repository import Gdk, Gtk, GLib
+import cairo
+
+class GridWindow(Gtk.Window):
+    def method_connect(self, event, cb):
+        def handler(obj, *args):
+            cb(*args)
+
+        print("Method_connect ran")
+        self.connect(event, handler)
+
+    def mouse_move(self, event):
+        #Changes event.x and event.y to be relative to the center.
+        x = event.x - self.drawing_area.window_shape[0] / 2
+        y = self.drawing_area.window_shape[1] / 2 - event.y
+        scale = self.drawing_area.get_current_scale()
+        event.x = x / scale + self.drawing_area.center[0]
+        event.y = y / scale + self.drawing_area.center[1]
+        self.drawing_area.mouse_move(event)
+        self.queue_draw()
+
+    def button_press(self, event):
+        print("button press activated")
+        o_x = event.x
+        o_y = event.y
+        x = event.x - self.drawing_area.window_shape[0] / 2
+        y = self.drawing_area.window_shape[1] / 2 - event.y
+        scale = 2 * self.drawing_area.get_current_scale()
+        event.x = x / scale + self.drawing_area.center[0]
+        event.y = y / scale + self.drawing_area.center[1]
+        self.drawing_area.do_button_press(event)
+        event.x = o_x
+        event.y = o_y
+
+    def key_press(self, event):
+        print("key press activated")
+        self.drawing_area.do_key_press(event, self.file_name_box.get_text())
+        self.queue_draw()
+
+    def configure(self, event):
+        print("configure activated")
+        self.drawing_area.window_shape = (event.width, event.height)
+
+    # handle submitting a constraint
+    def on_submit_click(self, widget):
+        self.drawing_area.inConstraint = int(self.constraint_box.get_text())
+        self.drawing_area.inValue = int(self.value_box.get_text())
+
+    def __init__(self):
+        Gtk.Window.__init__(self)
+
+        self.set_default_size(1.5 * SCREEN_SIZE, SCREEN_SIZE)
+
+        flowBox = Gtk.FlowBox()
+        flowBox.set_valign(Gtk.Align.START)
+        flowBox.set_selection_mode(Gtk.SelectionMode.NONE)
+
+        flowBox.set_valign(Gtk.Align.START)
+
+        self.add(flowBox)
+
+        container = Gtk.Fixed()
+        flowBox.add(container)
+
+        self.eventBox = Gtk.EventBox()
+        container.add(self.eventBox)
+
+        self.eventBox.set_events(Gdk.EventMask.BUTTON_PRESS_MASK
+                                 | Gdk.EventMask.BUTTON_RELEASE_MASK
+                                 | Gdk.EventMask.POINTER_MOTION_MASK
+                                 | Gdk.EventMask.SCROLL_MASK
+                                 | Gdk.EventMask.KEY_PRESS_MASK)
+
+        #add the graph box
+        self.drawing_area = GTK_Widget()
+        self.eventBox.add(self.drawing_area)
+
+        self.method_connect("key-release-event", self.key_press)
+        self.method_connect("button-release-event", self.button_press)
+        self.method_connect("configure-event", self.configure)
+        self.method_connect("motion_notify_event", self.mouse_move)
+
+        self.file_name_box = Gtk.Entry()
+        self.file_name_box.set_size_request(200, 40)
+
+        self.file_name_box.set_text("File")
+        self.file_name_box.set_editable(True)
+
+        container.put(self.file_name_box, 0, 0)
+
+        self.show_all()
+
+
+window = GridWindow()
+RunApp()
diff --git a/frc971/control_loops/python/spline_graph_usage.txt b/frc971/control_loops/python/spline_graph_usage.txt
new file mode 100644
index 0000000..a698298
--- /dev/null
+++ b/frc971/control_loops/python/spline_graph_usage.txt
@@ -0,0 +1,22 @@
+Spline Graph Usage
+
+Run with: bazel run spline_graph -c opt "YOUR SCREEN SIZE"
+
+On Start Place down first 6 points
+Then you go into editing mode, where you can drag your points around
+Then to create another spline, press p and the first three points will be autogenerated
+
+To export type in your file in the top right textfield ex.test.json
+It will be exported to spline_jsons/YOURFILE
+
+Notes:
+If importing gi, gi requires #!/usr/bin/python3 be at the top of the file
+
+This was built on top of BaseWindow
+
+Known Bugs:
+libspline throws errors about viable friction limits and viable voltage limits not overlapping
+bazel run lib_spline_test -c opt
+John commented those lines out
+
+Upon closing, reopens window
diff --git a/frc971/control_loops/python/spline_jsons/test.json b/frc971/control_loops/python/spline_jsons/test.json
new file mode 100644
index 0000000..09737e0
--- /dev/null
+++ b/frc971/control_loops/python/spline_jsons/test.json
@@ -0,0 +1 @@
+[[[1.6680953371273437, -3.4989921261100347], [1.8841223685193285, -3.3960504631707944], [2.0281410171518672, -3.272708402385707], [2.192647314562578, -3.1286520914802223], [2.3984302077132025, -3.0360669258966317], [2.5936689198479943, -2.912724865111545]], [[2.5936689198479943, -2.912724865111545], [2.788907631982786, -2.789382804326458], [2.973602163101745, -2.635283848339876], [3.190230535451773, -2.5732992795250538], [3.396013428602397, -2.511627935280236], [3.6117404172199103, -2.4705139150185405]]]
\ No newline at end of file
diff --git a/frc971/control_loops/python/spline_writer.py b/frc971/control_loops/python/spline_writer.py
new file mode 100644
index 0000000..df622f3
--- /dev/null
+++ b/frc971/control_loops/python/spline_writer.py
@@ -0,0 +1,72 @@
+import os
+import numpy as np
+
+
+class SplineWriter(object):
+    def __init__(self, namespaces=None, filename="auto_splines.cc"):
+        if namespaces:
+            self._namespaces = namespaces
+        else:
+            self._namespaces = ['frc971', 'autonomous']
+
+        self._namespace_start = '\n'.join(
+            ['namespace %s {' % name for name in self._namespaces])
+
+        self._namespace_end = '\n'.join([
+            '}  // namespace %s' % name for name in reversed(self._namespaces)
+        ])
+
+        self.filename_ = filename
+
+    def make_string_list(self, list):
+        string = "{{"
+        for i in range(0, len(list)):
+            if i == len(list) - 1:
+                string += str(list[i])
+            else:
+                string += str(list[i]) + ", "
+        return string + "}}"
+
+    def Write(self, spline_name, spline_idx, control_points):
+        """Writes the cc file to the file named cc_file."""
+        xs = control_points[:, 0]
+        ys = control_points[:, 1]
+        spline_count = (len(xs) - 6) / 5 + 1
+        with open(self.filename_, 'a') as fd:
+            fd.write(self._namespace_start)
+            #write the name
+            fd.write("\n\n::frc971::MultiSpline " + spline_name + "() {\n")
+            # write the objs, at the moment assumes a single constraint, needs fixing
+            fd.write(
+                "\t::frc971::MultiSpline spline;\n\t::frc971::Constraint constraints;\n"
+            )
+            fd.write(
+                "\tconstraints.constraint_type = 0;\n\tconstraints.value = 0;\n"
+            )
+            fd.write(
+                "\tconstraints.start_distance = 0;\n\tconstraints.end_distance = 0;\n"
+            )
+            fd.write('\n')
+            fd.write("\tspline.spline_idx = " + str(spline_idx) +
+                     ";\n\tspline.spline_count = " + str(spline_count) + ";\n")
+            fd.write("\tspline.spline_x = " + self.make_string_list(xs) +
+                     ";\n\tspline.spline_y = " + self.make_string_list(ys) +
+                     ";\n")
+            fd.write(
+                "\tspline.constraints = {{constraints}};\n\treturn spline;\n")
+            fd.write("}")
+            fd.write("\n\n")
+            fd.write(self._namespace_end)
+            fd.write("\n\n")
+
+
+def main():
+    writer = SplineWriter()
+    points = np.array([[1.0, 2.0], [3.0, 4.0], [5.0, 6.0], [7.0, 8.0],
+                       [9.0, 10.0], [11.0, 12.0]])
+    spline_name = "test_spline"
+    spline_idx = 1
+    writer.Write(spline_name, spline_idx, points)
+
+
+main()