diff --git a/y2018/control_loops/python/path_edit.py b/y2018/control_loops/python/path_edit.py
new file mode 100644
index 0000000..885b36c
--- /dev/null
+++ b/y2018/control_loops/python/path_edit.py
@@ -0,0 +1,360 @@
+from __future__ import print_function
+import os
+import basic_window
+import random
+import gi
+import numpy as np
+import scipy.spatial.distance
+gi.require_version('Gtk', '3.0')
+from gi.repository import Gdk
+import cairo
+
+import enum
+import csv # For writing to csv files
+
+from basic_window import OverrideMatrix, identity, quit_main_loop
+
+LENGTH_OF_FIELD = 323.65
+PIXELS_ON_SCREEN = 300
+
+def pxToIn(p):
+    return p*LENGTH_OF_FIELD/PIXELS_ON_SCREEN
+
+def inToPx(i):
+    return i*PIXELS_ON_SCREEN/LENGTH_OF_FIELD
+
+def px(cr):
+    return OverrideMatrix(cr, identity)
+
+def draw_px_cross(cr, x, y, length_px, r, g, b):
+    """Draws a cross with fixed dimensions in pixel space."""
+    cr.set_source_rgb(r, g, b)
+    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()
+
+def draw_px_x(cr, x, y, length_px1, r, g, b):
+    """Draws a x with fixed dimensions in pixel space."""
+    length_px = length_px1 / np.sqrt(2)
+    cr.set_source_rgb(r, g, b)
+    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()
+
+def draw_points(cr, p, size):
+    for i in range(0, len(p)):
+        draw_px_cross(cr, p[i][0], p[i][1], size, 0, np.sqrt(0.2 * i), 0)
+
+class Mode(enum.Enum):
+    kViewing = 0
+    kPlacing = 1
+    kEditing = 2
+    kExporting = 3
+    kImporting = 4
+
+def display_text(cr, text, widtha, heighta, widthb, heightb):
+    cr.scale(widtha, -heighta)
+    cr.show_text(text)
+    cr.scale(widthb, -heightb)
+
+# Create a GTK+ widget on which we will draw using Cairo
+class GTK_Widget(basic_window.BaseWindow):
+    def __init__(self):
+        super(GTK_Widget, self).__init__()
+
+        # 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
+
+        self.switch = True
+
+        # update list of control points
+        self.point_selected = False
+        # self.adding_spline = False
+        self.index_of_selected = -1
+        self.new_point = []
+
+        # For the editing mode
+        self.index_of_edit = -1 # Can't be zero beause array starts at 0
+        self.held_x = 0
+
+        # Theo take them from here?
+        self.selected_points = []
+
+        self.reinit_extents()
+
+    #John also wrote this
+    def add_point(self, x, y):
+        if(len(self.selected_points)<4):
+            self.selected_points.append([x,y])
+
+    """set extents on images, this needs to be redone with proper distances"""
+    def reinit_extents(self):
+        self.extents_x_min = -800
+        self.extents_x_max = 800
+        self.extents_y_min = -800
+        self.extents_y_max = 800
+
+    # this needs to be rewritten with numpy, i dont think this ought to have
+    # SciPy as a dependecy
+    def get_index_of_nearest_point(self):
+        cur_p = [[self.x, self.y]]
+        distances = scipy.spatial.distance.cdist(cur_p, self.all_controls)
+
+        return np.argmin(distances)
+
+    # return the closest point to the loc of the click event
+    def get_nearest_point(self):
+        return self.all_controls[self.get_index_of_nearest_point()]
+
+      # Handle the expose-event by updating the Window and drawing
+    def handle_draw(self, cr):
+        print(self.new_point)
+        print("SELF.POINT_SELECTED: " + str(self.point_selected))
+
+        # begin drawing
+        # Fill the background color of the window with grey
+        cr.set_source_rgb(0.5, 0.5, 0.5)
+        cr.paint()
+
+        # Draw a extents rectangle
+        cr.set_source_rgb(1.0, 1.0, 1.0)
+        cr.rectangle(self.extents_x_min, self.extents_y_min,
+                     (self.extents_x_max - self.extents_x_min),
+                     self.extents_y_max - self.extents_y_min)
+        cr.fill()
+
+        #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')
+
+        cr.set_source_rgb(0.3,0.3,0.3)
+        cr.rectangle(-150,-150,300,300)
+        cr.fill()
+        cr.set_source_rgb(0, 0, 0)
+        cr.rectangle(-150,-150,300,300)
+        cr.set_line_join(cairo.LINE_JOIN_ROUND)
+        cr.stroke()
+        cr.set_source_rgb(0, 0, 0)
+        cr.rectangle(inToPx(140-161.825),inToPx(76.575),inToPx(56),inToPx(-153.15))
+        cr.set_line_join(cairo.LINE_JOIN_ROUND)
+        cr.stroke()
+
+        cr.rectangle(inToPx(161.825-24),inToPx(90),inToPx(24),inToPx(-180))
+        cr.set_line_join(cairo.LINE_JOIN_ROUND)
+        cr.stroke()
+
+        cr.set_source_rgb(0.2, 0.2, 0.2)
+        cr.rectangle(inToPx(140-161.825),inToPx(76.575),inToPx(56),inToPx(-153.15))
+        cr.fill()
+
+        cr.rectangle(inToPx(161.825-24),inToPx(90),inToPx(24),inToPx(-180))
+        cr.fill()
+
+        # update all the things
+
+        if self.mode == Mode.kViewing:
+            cr.set_source_rgb(0, 0, 0)
+            cr.move_to(-300, 170)
+            cr.show_text("VIEWING")
+            cr.set_source_rgb(0.5, 0.5, 0.5)
+            # its gonna check for points_added from button_press_action
+            # The behavior of the click is such that it runs twice
+            # This is consistant with graph_edit.py which someone smart wrote
+            # So I'm just going to delete the last element in order to not get
+            # repeating points
+            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, 0,
+                                0, 0)
+                    cr.move_to(point[0], point[1]-15)
+                    display_text(cr, str(i), 0.5, 0.5, 2, 2)
+
+        if self.mode == Mode.kPlacing:
+            cr.set_source_rgb(0, 0, 0)
+            cr.move_to(-300, 170)
+            display_text(cr, "ADD", 1, 1, 1, 1)
+            cr.set_source_rgb(0.5, 0.5, 0.5)
+            # its gonna check for points_added from button_press_action
+            # The behavior of the click is such that it runs twice
+            # This is consistant with graph_edit.py which someone smart wrote
+            # So I'm just going to delete the last element in order to not get
+            # repeating points
+            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, 0,
+                                0, 0)
+                    cr.move_to(point[0], point[1]-15)
+                    display_text(cr, str(i), 0.5, 0.5, 2, 2)
+                    if(i==3):
+                        self.mode = Mode.kEditing
+
+        elif self.mode == Mode.kEditing:
+            cr.set_source_rgb(0, 0, 0)
+            cr.move_to(-300, 170)
+            display_text(cr, "EDITING", 1, 1, 1, 1)
+            cr.set_source_rgb(0.5, 0.5, 0.5)
+            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, 0,
+                                0, 0)
+                    cr.move_to(point[0], point[1]-15)
+                    display_text(cr, str(i), 0.5, 0.5, 2, 2)
+
+        elif self.mode == Mode.kExporting:
+            cr.set_source_rgb(0, 0, 0)
+            cr.move_to(-300, 170)
+            display_text(cr, "VIEWING", 1, 1, 1, 1)
+            cr.set_source_rgb(0.5, 0.5, 0.5)
+            #its gonna check for points_added from button_press_action
+
+            # The behavior of the click is such that it runs twice
+            # This is consistant with graph_edit.py which someone smart wrote
+            # So I'm just going to delete the last element in order to not get
+            # repeating points
+            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, 0,
+                                0, 0)
+                    cr.move_to(point[0], point[1]-15)
+                    display_text(cr, str(i), 0.5, 0.5, 2, 2)
+        elif self.mode == Mode.kImporting:
+            cr.set_source_rgb(0, 0, 0)
+            cr.move_to(-300, 170)
+            display_text(cr, "VIEWING", 1, 1, 1, 1)
+            cr.set_source_rgb(0.5, 0.5, 0.5)
+            # its gonna check for points_added from button_press_action
+
+            # The behavior of the click is such that it runs twice
+            # This is consistant with graph_edit.py which someone smart wrote
+            # So I'm just going to delete the last element in order to not get
+            # repeating points
+            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, 0,
+                                0, 0)
+                    cr.move_to(point[0], point[1]-15)
+                    display_text(cr, str(i), 0.5, 0.5, 2, 2)
+
+        cr.paint_with_alpha(.65)
+
+        draw_px_cross(cr, self.x, self.y, 10, 1, 0, 0)
+
+    def do_key_press(self, event):
+        keyval = Gdk.keyval_to_lower(event.keyval)
+        print("Gdk.KEY_" + Gdk.keyval_name(keyval))
+        if keyval == Gdk.KEY_q:
+            print("Found q key and exiting.")
+            quit_main_loop()
+        if keyval == Gdk.KEY_e:
+            self.mode = Mode.kExporting
+            # Will export to csv file
+            with open('points_for_pathedit.csv', mode='w') as points_file:
+                writer = csv.writer(points_file, delimiter=',', quotechar='"',
+                        quoting=csv.QUOTE_MINIMAL)
+                for item in self.selected_points:
+                    writer.writerow([str(item[0]), str(item[1])])
+                    print("Wrote: " + str(item[0]) + " " +  str(item[1]))
+        if keyval == Gdk.KEY_i:
+            self.mode = Mode.kImporting
+            # import from csv file
+            self.selected_points = []
+            with open('points_for_pathedit.csv') as points_file:
+                reader = csv.reader(points_file, delimiter=',')
+                for row in reader:
+                    self.add_point(float(row[0]), float(row[1]))
+                    print("Added: " + row[0] + " " +  row[1])
+
+        self.redraw()
+
+    def button_press_action(self):
+        if self.switch:
+            self.switch = False
+            if self.mode == Mode.kPlacing:
+                #Check that the point clicked is on the field
+                if(self.x<150 and self.x>-150 and self.y <150 and self.y >-150):
+                    self.add_point(self.x, self.y)
+            if self.mode == Mode.kEditing:
+                # Now after index_of_edit is not -1, the point is selected, so
+                # user can click for new point
+                    print("INDEX OF EDIT: " + str(self.index_of_edit))
+
+                    if self.index_of_edit > -1 and self.held_x != self.x:
+                        print("INDEX OF EDIT: " + str(self.index_of_edit))
+                        self.selected_points[self.index_of_edit] = [self.x, self.y]
+                        self.index_of_edit = -1
+                    else:
+                        print("mode == 2")
+                        # Get clicked point
+                        # Find nearest
+                        # Move nearest to clicked
+                        cur_p = [self.x, self.y]
+                        print("CUR_P: " + str(self.x) + " " + str(self.y))
+                        # What I wanna do is get each point
+                        # Get the distance between each for x and y
+                        # Save the index of the point closest
+                        nearest = 1000
+                        index = 0
+                        for ind, i in enumerate(self.selected_points):
+                            # pythagorean
+                            distance = np.sqrt((cur_p[0] - i[0])**2 + (cur_p[1] - i[1])**2)
+                            if distance < nearest:
+                                nearest = distance
+                                index = ind
+                                print("Nearest: " + str(nearest))
+                                print("Index: " + str(index))
+                                self.index_of_edit = index
+                                self.held_x = self.x
+        else:
+            self.switch = True
+
+        self.redraw()
+
+
+    def do_button_press(self, event):
+        print("button press activated")
+        self.x = event.x
+        self.y = event.y
+        self.button_press_action()
+
+
+
+silly = GTK_Widget()
+basic_window.RunApp()
diff --git a/y2018/control_loops/python/spline_generate.py b/y2018/control_loops/python/spline_generate.py
new file mode 100644
index 0000000..4376f37
--- /dev/null
+++ b/y2018/control_loops/python/spline_generate.py
@@ -0,0 +1,366 @@
+#!/usr/bin/python
+import numpy as np
+import matplotlib.pyplot as plt
+from frc971.control_loops.python import drivetrain
+
+# used to define properties of the drivetrain, changes depending on robot
+# see yXXXX/control_loops/python/drivetrain.py for the current values
+
+kDrivetrain = drivetrain.DrivetrainParams(
+    J = 6.0,
+    mass=68.0,
+    robot_radius=0.616 / 2.0,
+    wheel_radius=0.127 / 2.0 * 120.0 / 118.0,
+    G_low=46.0 / 60.0 * 20.0 / 48.0 * 14.0 / 62.0,
+    G_high=62.0 / 44.0 * 20.0 / 48.0 * 14.0 / 62.0,
+    q_pos_low=0.12,
+    q_pos_high=0.14,
+    q_vel_low=1.0,
+    q_vel_high=0.95,
+    efficiency=0.70,
+    has_imu=True,
+    force=True,
+    kf_q_voltage=13.0,
+    controller_poles=[0.82, 0.82],
+)
+
+drivetrain = drivetrain.Drivetrain(kDrivetrain)
+# set up coefficients for Hemrite basis function evaluation
+coeffs = np.array([[1, 0, 0, -10, 15, -6], [0, 1, 0, -6, 8, -3], [0, 0, 0.5, -1.5, 1.5, -0.5], [0, 0, 0, 0.5, -1, 0.5], [0, 0, 0, -4, 7, -3], [0, 0, 0, 10, -15, 6]])
+coeffs_prime = np.empty_like(coeffs)
+for ii in range(0, len(coeffs)):
+    for jj in range(0, len(coeffs[ii]) - 1):
+        coeffs_prime[ii][jj] = (jj + 1) * coeffs[ii][jj]
+
+def RungeKutta(f, x, dt):
+    """4th order RungeKutta integration of F starting at X."""
+    a = f(x)
+    b = f(x + dt / 2.0 * a)
+    c = f(x + dt / 2.0 * b)
+    d = f(x + dt * c)
+
+    return x + dt * (a + 2.0 * b + 2.0 * c + d) / 6.0
+
+def normalize(v):
+    norm = np.linalg.norm(v)
+    return v / norm
+
+def theta(v):
+    return np.arctan2(v[1], v[0])
+
+# evaluate Nth hermite basis function at t
+def nth_H(N, t):
+    return coeffs[N][0] + coeffs[N][1]*t + coeffs[N][2]*t**2 + coeffs[N][3]*t**3 + coeffs[N][4]*t**4 + coeffs[N][5]*t**5
+
+def nth_H_prime(N, t):
+    return coeffs[N][0] + coeffs[N][1]*t + coeffs[N][2]*t**2 + coeffs[N][3]*t**3 + coeffs[N][4]*t**4 
+
+# class defining a quintic Hermite spline, with utilities for modification and plotting
+class Hermite_Spline:
+    # init method given known parameters, ie savefile loading(if necessary)
+    def __init__(self, start, control1, control2, end, resolution = 200):
+        self.start = start
+        self.end = end
+        self.control1 = control1
+        self.control2 = control2
+
+        self.points = np.array([])
+        self.velocities = []
+        self.accelerations = []
+        self.arc_lengths = []
+        self.thetas = []
+        self.omegas = []
+        self.curvatures = []
+
+        self.shifted_points = []
+
+        self.Ks = []
+        self.dKs = []
+
+        # coefficients are po, v0, a0, a1, v1, p1
+        self.coeffs = np.array([])
+        self.compute_coefficients()
+        self.resolution = resolution
+        self.setup()
+
+    # take paramters and compute coeffcicents for Hermite basis functions, to be called every time he change control points
+    def compute_coefficients(self):
+        self.coeffs = np.append(self.coeffs, np.array(self.start))
+        self.coeffs = np.append(self.coeffs, np.array(self.control1) - np.array(self.start))
+        self.coeffs = np.append(self.coeffs, [0,0])
+        self.coeffs = np.append(self.coeffs, [0,0])
+        self.coeffs = np.append(self.coeffs, np.array(self.end) - np.array(self.control2))
+        self.coeffs = np.append(self.coeffs, np.array(self.end))
+
+        self.coeffs = np.reshape(self.coeffs, newshape = (6, 2))
+
+    # setters for control points, set coefficients
+    def set_positions(self, p1 = None, p2 = None):
+        if p1 != None:
+            self.start = p1
+        if p2 != None:
+            self.end = p2
+
+        self.compute_coefficients()
+
+    def set_controls(self, c1 = None, c2 = None):
+        if c1 != None:
+            self.control1 = c1
+        if c2 != None:
+            self.control2 = c2
+
+        self.compute_coefficients()
+
+    def set_velocities(self, v1 = None, v2 = None):
+        if v1 != None:
+            self.control1 = self.start + v1
+        if v2 != None:
+            self.control2 = self.end + v2
+        
+        self.compute_coefficients()
+
+    def get_smoothness(self):
+        K = self.get_curvature()
+        return np.sum(np.abs(np.gradient(K)))
+
+    # given Basis functions and controls compute coordinate given t
+    def spline_eval_hermite(self, t):
+        return np.array(self.coeffs[0]*nth_H(0, t) + self.coeffs[1]*nth_H(1, t)+ self.coeffs[2]*nth_H(2, t) + self.coeffs[3]*nth_H(3, t) + self.coeffs[4]* nth_H(4, t)+ self.coeffs[5]*nth_H(5, t))
+    
+    # given Basis functions and controls compute velocity given t
+    def spline_eval_hermite_v(self, t):
+         return normalize(np.array(self.coeffs[0]*nth_H_prime(0, t) + self.coeffs[1]*nth_H_prime(1, t)+ self.coeffs[2]*nth_H_prime(2, t) + self.coeffs[3]*nth_H_prime(3, t) + self.coeffs[4]* nth_H_prime(4, t)+ self.coeffs[5]*nth_H_prime(5, t)))
+
+    # take coefficients and compute spline points/properties
+    def setup(self, resolution_multiplier = 10, dt = .000001):
+        points = []
+        velocities = []
+        accelerations = []
+        s = []
+        thetas = []
+        omegas = []
+        curvatures = []
+
+        last_point = self.spline_eval_hermite(0)
+        distance = 0
+
+        # iterate through interim points and compute pos_vectors, and at predefined points arc length,
+        # velocity, and acceleration vectors and store them at their associated index 
+        for i in range(0, self.resolution * resolution_multiplier):
+            t = i / (1.0 * self.resolution * resolution_multiplier)
+
+            current_point = self.spline_eval_hermite(t)
+            current_point_dt = self.spline_eval_hermite(t + dt)
+            current_s = np.linalg.norm(current_point - last_point)
+
+            ds = np.linalg.norm(current_point_dt - current_point)
+
+            distance = current_s + distance
+            # at important points compute important values and store
+            if i % resolution_multiplier  == 0:
+                s.append(distance)
+                points.append(current_point)
+
+                v = self.spline_eval_hermite_v(t)
+                v_dt = self.spline_eval_hermite_v(t + dt)
+                theta_t = theta(v)
+                theta_dt = theta(v_dt)
+
+                a = (v_dt - v) / ds
+                omega = (theta_dt - theta_t) / ds
+                if np.linalg.norm(v) == 0:
+                    curvature = 0
+                else:
+                    curvature = np.linalg.det(np.column_stack((v, a)) / (np.linalg.norm(v)**(3/2)))
+
+                velocities.append(v)
+                accelerations.append(a)
+                thetas.append(theta_t)
+                omegas.append(omega)
+                if curvature == 0:
+                    curvatures.append(0.0001)
+                else:
+                    curvatures.append(curvature)
+ 
+            last_point = current_point
+
+        self.arc_lengths = np.array(s)
+        self.points = np.reshape(points, newshape = (-1, 2))
+        self.velocities = np.reshape(velocities, newshape = (-1, 2))
+        self.accelerations = np.reshape(accelerations, newshape = (-1, 2))
+        self.thetas = np.array(thetas)
+        self.omegas = np.array(omegas)
+        self.curvatures = np.array(curvatures)
+
+
+    def plot_diagnostics(self):
+        plt.figure("Spline")
+        plt.title('Spline')
+        plt.plot(self.points[:, 0], self.points[:, 1])
+        # plt.scatter(self.points[:, 0], self.points[:, 1])
+
+        plt.figure("Diagnostics")
+
+        plt.subplot(2, 2, 1)        
+        plt.title('theta')
+        plt.xlabel('arc_length')
+        plt.ylabel('theta')
+        theta, = plt.plot(self.arc_lengths, self.thetas, label = 'theta')
+        plt.legend(handles = [theta])
+
+        plt.subplot(2, 2, 2)
+        plt.title('omegas')
+        plt.xlabel('arc_length')
+        plt.ylabel('omega')
+        omega, = plt.plot(self.arc_lengths, self.omegas, label = 'omega')
+        plt.legend(handles = [omega])
+
+        plt.subplot(2, 2, 3)
+        plt.title('Velocities')
+        plt.xlabel('arc_length')
+        plt.ylabel('velocity')
+        dxds, = plt.plot(self.arc_lengths, self.velocities[:, 0], label = 'dx/ds')
+        dyds, = plt.plot(self.arc_lengths, self.velocities[:, 1], label = 'dy/ds')
+        plt.legend(handles = [dxds, dyds])
+
+        plt.subplot(2, 2, 4)
+        plt.title('Accelerations')
+        plt.xlabel('arc_length')
+        plt.ylabel('acceleration')
+        dx2ds2, = plt.plot(self.arc_lengths, self.accelerations[:, 0], label = 'd^2x/ds^2')
+        dy2ds2, = plt.plot(self.arc_lengths, self.accelerations[:, 1], label = 'd^2x/ds^2')
+        plt.legend(handles = [dx2ds2, dy2ds2])
+
+# class defining a number of splines with convinience methods
+class Path:
+    def __init__(self):
+        self.splines = []
+        self.knot_accels = []
+
+    def add_spline(self, spline):
+        self.splines.append(spline)
+
+    def get_K(self):
+        curvatures = []
+        for spline in self.splines:
+            curvatures.append(spline.curvatures)
+        return np.array(curvatures).flatten()
+
+    def get_S(self):
+        arc_lengths = []
+        for spline in self.splines:
+            arc_lengths.append(spline.arc_lengths)
+        return np.array(arc_lengths).flatten()
+
+    def get_points(self):
+        points = []
+        for spline in self.splines:
+            points.append(spline.points)
+        return points
+
+    def get_velocities(self, i):
+        velocities = []
+        for spline in self.splines:
+            velocities.append(spline.points)
+        return velocities
+
+    def remove_spine(self, i):
+        if i < len(self.splines):
+            self.splines.pop(i)
+        else:
+            print("index %f out of bounds, no spline of that index" % i)
+
+    def join(self, first_priority = False):
+        for i in range(0, len(self.splines)):
+            if first_priority & i != len(self.splines):
+                print("unfinished")
+
+
+# class which takes a Path object along with constraints and reparamterizes it with respect to time
+class Trajectory:
+    def __init__(self, path, max_angular_accel=3, max_voltage=11, max_normal_accel = .2):
+        self.path = path
+        self.A = drivetrain.A_continuous
+        self.B = drivetrain.B_continuous
+        self.robot_radius = drivetrain.robot_radius
+        self.Kv = 100
+        self.robot_radius = 3
+        self.max_angular_accel = max_angular_accel
+        self.max_voltage = max_voltage
+        self.max_normal_accel = max_normal_accel
+
+        self.max_velocities_adhering_to_normal_accel = []
+        self.max_velocities_adhering_to_voltage = []
+        self.path.splines[0].setup(resolution_multiplier = 100)
+
+        self.set_max_v_adhering_to_normal_accel()
+        self.max_voltageK_pass()
+
+    def set_max_v_adhering_to_normal_accel(self):
+        Ks = self.path.get_K()
+        accels = np.full_like(Ks, fill_value = self.max_normal_accel)
+        max_velocities = np.sqrt(np.abs(accels / Ks))
+        self.max_velocities_adhering_to_normal_accel = max_velocities
+
+    def max_voltageK_pass(self):
+        max_ds_dt = []
+        Ks = self.path.get_K()
+        turning_radii = np.full_like(Ks, fill_value = 1) / np.abs(Ks)
+
+    
+
+        # compute max steady-state velocity given voltage constraints  
+        for i in range(0, len(Ks)):
+            v_ratio = (turning_radii[i] + self.robot_radius) / (turning_radii[i] - self.robot_radius)
+            matrix = np.array([[self.A[1, 1], self.A[1, 3], self.B[1, 1]], [self.A[3, 1] - 1, self.A[3, 3], self.B[3, 1]], [-1, v_ratio, 0]])
+            sols = np.array([-1 * self.max_voltage * self.B[1, 0], -1 * self.max_voltage * self.B[3, 0], 0])
+            Vs = np.dot(np.linalg.inv(matrix), sols)
+            max_ds_dt.append((Vs[0] + Vs[1]) / 2)
+            
+        self.max_velocities_adhering_to_voltage = max_ds_dt
+    # compute the maximum acceleration we can ask for given voltage and, ya know, staying on the path.
+
+
+    '''
+    These methods use the continuous form of our drivetrain state equation
+    in order to compute the maximum acceleration which adheres to the path
+    and voltage constraints, as well as any arbitary set of constraints
+    on velocity as a function of arc_length
+    '''
+    
+    def forward_accel_pass(self):
+        points = self.path.get_points()
+        velocities = self.path.get_velocities()
+        curvatures = self.path.get_K()
+        arc_lenghts = self.path.get_S()
+
+        for i in range(0, len(points)):
+            Xn1 =   
+        
+    
+    def backward_accelaration_pass(self):
+
+        print("max backward accel pass")    
+
+    
+    def plot_diagnostics(self, i = 0):
+
+        plt.figure('max velocity')
+        plt.title('max_v_normal_accel')
+        plt.xlabel('arc_length')
+        plt.ylabel('max V')
+        max_v_normal = plt.plot(self.path.get_S(), self.max_velocities_adhering_to_normal_accel, label = 'ds/dt (normal)')#   , label = 'ds/dt')
+        curvature = plt.plot(self.path.get_S(), 1000 * np.abs(self.path.get_K()), label = 'K')
+        max_v_K_V = plt.plot(self.path.get_S(), self.max_velocities_adhering_to_voltage, label = 'ds/dt (voltage)')
+        plt.legend(handles = [max_v_normal[0], curvature[0], max_v_K_V[0]])
+
+def main():
+    A = Hermite_Spline(np.array([0,0]), np.array([0,400]), np.array([200,300]), np.array([200,200]), resolution = 200)
+    A.plot_diagnostics()
+    path = Path()
+    path.add_spline(A)
+    trajectory = Trajectory(path, 0)
+    trajectory.plot_diagnostics()
+    plt.show()
+
+main()