diff --git a/lookAhead.py b/lookAhead.py
index d90ad0d..7f58614 100644
--- a/lookAhead.py
+++ b/lookAhead.py
@@ -1,14 +1,8 @@
 #!/usr/bin/env python
 
-import sys  # for redirecting output in bash, could be removed
-# import time # for sleeping - time.sleep is commented out below right now
-import os
+import sys
 import rospy
-import argparse
-import subprocess
-from subprocess import Popen
 import psutil
-import time
 from geometry_msgs.msg import Twist
 import numpy as np
 import keras
@@ -22,27 +16,14 @@
 v = 0.0
 yaw = 0.0
 
-environments = {0.009: "ice_009",
-                0.09: "ice_09",
-                0.9: "ice_9",
-                1: "control",
-                1000: "mud",
-                0.05: "ice_05",
-                0.5: "ice_5",
-                0.02: "ice_02",
-                0.2: "ice_2",
-                0.07: "ice_07",
-                0.7: "ice_7"}
-
 model = keras.models.load_model('/home/bezzo/catkin_ws/src/capstone_nodes/NNet_all_tf_210.h5', custom_objects={
     'Normalization': keras.layers.experimental.preprocessing.Normalization()})
 
 def statesCallback(data):
     global x, y, v, yaw
-    # find index of slash
+    # find index of jackal
     name = data.name
     index = name.index("jackal")
-    # index = name.index("/")
     x = data.pose[index].position.x
     y = data.pose[index].position.y
     v = data.twist[index].linear.x
@@ -56,51 +37,37 @@ def statesCallback(data):
     yaw = euler[2]
 
 
-def terminate_process_and_children(p):
-    import psutil
-    process = psutil.Process(p.pid)
-    for sub_process in process.children(recursive=True):
-        sub_process.send_signal(signal.SIGINT)
-    # p.wait()  # we wait for children to terminate
-    p.terminate()
-
-
-def signal_process_and_children(pid, signal_to_send, wait=False):
-    process = psutil.Process(pid)
-    for children in process.children(recursive=True):
-        if signal_to_send == 'suspend':
-            children.suspend()
-        elif signal_to_send == 'resume':
-            children.resume()
-        else:
-            children.send_signal(signal_to_send)
-    if wait:
-        process.wait()
-
-
-def terminate_ros_node():
-    list_cmd = subprocess.Popen("rosnode list", shell=True, stdout=subprocess.PIPE)
-    list_output = list_cmd.stdout.read()
-    retcode = list_cmd.wait()
-    assert retcode == 0, "List command returned %d" % retcode
-    for str in list_output.split("\n"):
-        # if (str.startswith(s)):
-        os.system("rosnode kill " + str)
+def calculate_mu(run):
+    if run <= 120:
+        return 0.009
+    elif 120 < run <= 240:
+        return 0.09
+    elif 240 < run <= 360:
+        return 1
+    elif 360 < run <= 480:
+        return 0.05
+    elif 480 < run <= 600:
+        return 0.5
+    elif 600 < run <= 720:
+        return 0.02
+    elif 720 < run <= 840:
+        return 0.2
+    elif 840 < run <= 960:
+        return 0.07
+    elif 960 < run:
+        return 0.7
+    return None
 
 
 def robotUnsafe(robx, roby, path):
-    # print("hi")
     safety_tolerance = 2
     dists = [0] * len(path)
     i = 0
     for point in path:
         dists[i] = sqrt(pow((point[0] - robx), 2) + pow((point[1] - roby), 2))
-        # print(i)
         i = i + 1
-    # print(dists),
     val = min(dists)
     closest_index = dists.index(val)
-    # print(val)
     return val > safety_tolerance, val, closest_index
 
 
@@ -130,7 +97,6 @@ def getLookAheadPoint(waypoints, robx, roby, lookAheadDistance, lastIndex, lastF
         discriminant = sqrt(discriminant)
         t1 = (-b - discriminant) / (2 * a)
         t2 = (-b + discriminant) / (2 * a)
-        # print('t guys', t1, t2)
         if 0 <= t1 <= 1 and j + t1 > lastFractionalIndex:
             return (E[0] + t1 * d[0], E[1] + t1 * d[1]), j, j + t1
         if 0 <= t2 <= 1 and j + t2 > lastFractionalIndex:
@@ -155,10 +121,8 @@ def injectPoints(waypoints):
         vector = (vector[0] / d * spacing, vector[1] / d * spacing)
         for i in range(0, num_points_that_fit):
             new_list = (start_point[0] + vector[0] * i, start_point[1] + vector[1] * i)
-            # print(new_list)
             new_points.append(new_list)
         new_points.append(end_point)
-    # print(new_points)
     return new_points
 
 
@@ -181,10 +145,10 @@ def smoothPath(path):  # path is [(x1, y1), ..., (xend, yend)]
 
 
 def vel_pid(vg, vc, dt):
-    v_goal = vg # output of neural net in m/s
-    v_curr = vc # current velocity in m/s
+    v_goal = vg  # output of neural net in m/s
+    v_curr = vc  # current velocity in m/s
     prev_err = 0.0
-    windup_guard = 10 # needs to be changed??
+    windup_guard = 10  # needs to be changed??
     kp = 1
     ki = 0.1
     kd = 0.1
@@ -196,7 +160,7 @@ def vel_pid(vg, vc, dt):
         i = windup_guard
     elif i > windup_guard:
         i = windup_guard
-    d = 0.0 # use acceleration calculated above??
+    d = 0.0  # use acceleration calculated above??
     if delta_time > 0:
         d = delta_error/dt
     prev_err = error
@@ -204,59 +168,42 @@ def vel_pid(vg, vc, dt):
     return vel
 
 
-# def main(velocity, angle_deg, log_file, run_num):
-def main(velocity, angle_deg, run_num):
+def stop_robot(vel_msg, velocity_publisher):
+    vel_msg.linear.x = 0
+    vel_msg.angular.z = 0
+    velocity_publisher.publish(vel_msg)
+
+
+def main(angle_deg):
     velocity_publisher = rospy.Publisher('/jackal_velocity_controller/cmd_vel', Twist, queue_size=10)
-    # velocity_publisher = rospy.Publisher('/husky_velocity_controller/cmd_vel', Twist, queue_size=10)
     rospy.Subscriber('/gazebo/model_states', ModelStates, statesCallback)
-    info = "{lin_vel}, {ang_vel}, {angle}, {deviation}\n"
     rate = rospy.Rate(10)
     vel_msg = Twist()
     angle = radians(angle_deg)  # in radians
     branching_point = (10, 0)
     end_point = (branching_point[0] + 10 * cos(angle), 10 * sin(angle))
     print(end_point)
-    # waypoints = [(10, 0), (0, 10), (10, 10), (0, 0)]
     waypoints = [branching_point, end_point]
     waypoints2 = [(0, 0), branching_point, end_point]
-    path = injectPoints(waypoints2)
     lookAheadDistance = 10
     horizon = 10
     lastIndex = 0
-    # lastLookAheadIndex = 0
     lastFractionalIndex = 0
     lookAheadPoint = waypoints[0]
     atGoalHack = 0  # needs to be fixed
     start_angle = yaw
-    # i = 0
-    fut_velocity = 0.5
-
-    # bag_location = "bagfiles/{env}/trainingData".format(env=environments[mu]) + run_num
-    # command = "rosbag record -O " + bag_location + " /gazebo/model_states /odometry/filtered /imu/data"
-    # proc = subprocess.Popen(command, stdin=subprocess.PIPE, shell=True, executable='/bin/bash')
 
     while not rospy.is_shutdown():
         path = injectPoints(waypoints2)
         unsafe, robot_deviation, closest_index = robotUnsafe(x, y, path)
         if unsafe:
             print("unsafe")
-            vel_msg.linear.x = 0
-            vel_msg.angular.z = 0
-            velocity_publisher.publish(vel_msg)
-            # log_file.write(info.format(lin_vel=0, ang_vel=0, angle=angle_deg,
-            #                           deviation=robot_deviation))
+            stop_robot(vel_msg, velocity_publisher)
             break
-        # print(unsafe)
-        # print(robot_deviation)
 
         if robotAtGoal(x, y, waypoints[-1][0], waypoints[-1][1]) and lastIndex == len(waypoints) - 1:
-            # if robotAtGoal(x, y, waypoints[-1][0], waypoints[-1][1]) and lastIndex == len(waypoints) - 1 and atGoalHack>100:
             print("at goal:", x, y)
-            vel_msg.linear.x = 0
-            vel_msg.angular.z = 0
-            velocity_publisher.publish(vel_msg)
-            # log_file.write(info.format(lin_vel=0, ang_vel=0, angle=angle_deg,
-            #                           deviation=robot_deviation))
+            stop_robot(vel_msg, velocity_publisher)
             break
 
         lookAheadPoint, lastIndex, lastFractionalIndex = getLookAheadPoint(waypoints, x, y, lookAheadDistance,
@@ -265,14 +212,12 @@ def main(velocity, angle_deg, run_num):
         goal_pose_x = lookAheadPoint[0]
         goal_pose_y = lookAheadPoint[1]
 
-        # mu = mu[horizon] ## This is just a general concept for when we don't have a constant mu
         horizon_point1 = path[closest_index + horizon]
         horizon_point2 = path[closest_index + horizon + 1]
         # Estimate angle from starting pose
         angl = atan2(horizon_point2[1] - horizon_point1[1], horizon_point2[0] - horizon_point1[0]) - start_angle
-        # estimate angle from current pose
-        #angl = atan2(horizon_point2[1] - horizon_point1[1], horizon_point2[0] - horizon_point1[0]) - yaw
         fut_velocity = model.predict([[mu, angl]])[0][0]
+
         # linear velocity in the x-axis:
         vel_msg.linear.x = fut_velocity
         vel_msg.linear.y = 0
@@ -288,22 +233,11 @@ def main(velocity, angle_deg, run_num):
         rate.sleep()
         atGoalHack += 1
 
-        # writing to the log file
-        # log_file.write(info.format(lin_vel=vel_msg.linear.x, ang_vel=vel_msg.angular.z,
-        #                           angle=angle_deg, deviation=robot_deviation))
-
-    # log_file.close()
     print("kill me")
     sys.stdout.flush()
     # time.sleep(20)
     raw_input("")  # kill 0 sent from bash script not working, so you have to ctrl-c manually
 
-    # terminate_process_and_children(proc)
-    # signal_process_and_children(proc.pid, signal.SIGINT, True)
-    # terminate_ros_node()
-    # proc.send_signal(subprocess.signal.SIGINT)
-    # proc.kill()
-    # proc.terminate()
     for process in psutil.process_iter():
         print(process.cmdline())
 
@@ -311,46 +245,8 @@ def main(velocity, angle_deg, run_num):
 if __name__ == "__main__":
     rospy.init_node('capstone_nodes', anonymous=True)
 
-    # get run number
     run = rospy.get_param('~run')
     angle = rospy.get_param('~angle')
-    run_num = str(run)
-
-    # automatically calculate angle
-    # automatically calculate mu
-    mu = 0
-    if run <= 120:
-        mu = 0.009
-    elif 120 < run <= 240:
-        mu = 0.09
-    elif 240 < run <= 360:
-        mu = 1
-    elif 360 < run <= 480:
-        mu = 0.05
-    elif 480 < run <= 600:
-        mu = 0.5
-    elif 600 < run <= 720:
-        mu = 0.02
-    elif 720 < run <= 840:
-        mu = 0.2
-    elif 840 < run <= 960:
-        mu = 0.07
-    elif 960 < run:
-        mu = 0.7
-
-    # use neural network to choose velocity
-
-    # velocity = model.predict([[mu, angle]])[0][0]
-    # velocity = 1.0
-    env = environments[mu]
-
-    # bag_location = "bagfiles/trainingData" + args.run_num
-
-    # log_file = "../logs/{run}_{env}_{vel}_{angle}.txt".format(run=run_num, env=env, vel=str(velocity), angle=str(angle)) # this needs to be fixed, right now you run test.sh from the bagfiles directory
-    # file format: velocity, angle, path_deviation
-    # file = open(log_file, "w")
+    mu = calculate_mu(run)
 
-    print("velocity: ", velocity, "angle: ", angle)
-    # print(angle, args.angle)
-    # main(velocity, angle, file, run_num)
-    main(velocity, angle, run_num)
+    main(angle)
diff --git a/pure_pursuit_cap.py b/pure_pursuit_cap.py
index d4fd4d0..0c481a7 100644
--- a/pure_pursuit_cap.py
+++ b/pure_pursuit_cap.py
@@ -5,7 +5,7 @@
 import numpy as np
 from nav_msgs.msg import Odometry
 from gazebo_msgs.msg import ModelStates
-from math import pow, atan2, sqrt, ceil, sin, cos, pi
+from math import pow, atan2, sqrt, ceil, sin, cos, pi, radians
 from tf.transformations import euler_from_quaternion
 
 x = 0.0
@@ -16,10 +16,9 @@
 
 def statesCallback(data):
     global x, y, v, yaw
-    # find index of slash
+    # find index of jackal
     name = data.name
     index = name.index("jackal")
-    # index = name.index("/")
     x = data.pose[index].position.x
     y = data.pose[index].position.y
     v = data.twist[index].linear.x
@@ -38,7 +37,7 @@ def robotUnsafe(robx, roby, path):
     dists = [0]*len(path)
     i = 0
     for point in path:
-        dists[i] = qrt(pow((point[0] - robx), 2) + pow((point[1] - roby), 2))
+        dists[i] = sqrt(pow((point[0] - robx), 2) + pow((point[1] - roby), 2))
     val = min(dists)
     return val > safety_tolerance
 
@@ -117,37 +116,36 @@ def smoothPath(path):  # path is [(x1, y1), ..., (xend, yend)]
     return newPath
 
 
+def stop_robot(vel_msg, velocity_publisher):
+    vel_msg.linear.x = 0
+    vel_msg.angular.z = 0
+    velocity_publisher.publish(vel_msg)
+
+
 def main():
     rospy.init_node('pure_pursuit_cap', anonymous=True)
     velocity_publisher = rospy.Publisher('/jackal_velocity_controller/cmd_vel', Twist, queue_size=10)
-    # velocity_publisher = rospy.Publisher('/husky_velocity_controller/cmd_vel', Twist, queue_size=10)
     rospy.Subscriber('/gazebo/model_states', ModelStates, statesCallback)
     rate = rospy.Rate(10)
     vel_msg = Twist()
     angle = radians(20)
     branching_point = (10, 0)
     end_point = (branching_point[0] + 10*cos(angle), 10*sin(angle))
-    #waypoints = [(10, 0), (0, 10), (10, 10), (0, 0)]
     waypoints = [branching_point, end_point]
     path = injectPoints(waypoints)
     lookAheadDistance = 2
     lastIndex = 0
-    lastLookAheadIndex = 0
     lastFractionalIndex = 0
     lookAheadPoint = waypoints[0]
-    i = 0
+
     while not rospy.is_shutdown():
 
         if robotUnsafe (x, y, path):
-            vel_msg.linear.x = 0
-            vel_msg.angular.z = 0
-            velocity_publisher.publish(vel_msg)
+            stop_robot(vel_msg, velocity_publisher)
             break
 
         if robotAtGoal(x, y, waypoints[-1][0], waypoints[-1][1]) and lastIndex == len(waypoints) - 1:
-            vel_msg.linear.x = 0
-            vel_msg.angular.z = 0
-            velocity_publisher.publish(vel_msg)
+            stop_robot(vel_msg, velocity_publisher)
             break
 
         lookAheadPoint, lastIndex, lastFractionalIndex = getLookAheadPoint(waypoints, x, y, lookAheadDistance,
@@ -173,4 +171,4 @@ def main():
 
 
 if __name__ == "__main__":
-    main()
\ No newline at end of file
+    main()
diff --git a/src/lookAhead.py b/src/lookAhead.py
index d7f9caa..5bcb52f 100755
--- a/src/lookAhead.py
+++ b/src/lookAhead.py
@@ -1,14 +1,8 @@
 #!/usr/bin/env python
 
-import sys  # for redirecting output in bash, could be removed
-# import time # for sleeping - time.sleep is commented out below right now
-import os
+import sys
 import rospy
-import argparse
-import subprocess
-from subprocess import Popen
 import psutil
-import time
 from geometry_msgs.msg import Twist
 import numpy as np
 import keras
@@ -22,27 +16,15 @@
 v = 0.0
 yaw = 0.0
 
-environments = {0.009: "ice_009",
-                0.09: "ice_09",
-                0.9: "ice_9",
-                1: "control",
-                1000: "mud",
-                0.05: "ice_05",
-                0.5: "ice_5",
-                0.02: "ice_02",
-                0.2: "ice_2",
-                0.07: "ice_07",
-                0.7: "ice_7"}
-
 model = keras.models.load_model('/home/bezzo/catkin_ws/src/capstone_nodes/NNet_all_tf_210.h5', custom_objects={
     'Normalization': keras.layers.experimental.preprocessing.Normalization()})
 
+
 def statesCallback(data):
     global x, y, v, yaw
-    # find index of slash
+    # find index of jackal
     name = data.name
     index = name.index("jackal")
-    # index = name.index("/")
     x = data.pose[index].position.x
     y = data.pose[index].position.y
     v = data.twist[index].linear.x
@@ -56,36 +38,26 @@ def statesCallback(data):
     yaw = euler[2]
 
 
-def terminate_process_and_children(p):
-    import psutil
-    process = psutil.Process(p.pid)
-    for sub_process in process.children(recursive=True):
-        sub_process.send_signal(signal.SIGINT)
-    # p.wait()  # we wait for children to terminate
-    p.terminate()
-
-
-def signal_process_and_children(pid, signal_to_send, wait=False):
-    process = psutil.Process(pid)
-    for children in process.children(recursive=True):
-        if signal_to_send == 'suspend':
-            children.suspend()
-        elif signal_to_send == 'resume':
-            children.resume()
-        else:
-            children.send_signal(signal_to_send)
-    if wait:
-        process.wait()
-
-
-def terminate_ros_node():
-    list_cmd = subprocess.Popen("rosnode list", shell=True, stdout=subprocess.PIPE)
-    list_output = list_cmd.stdout.read()
-    retcode = list_cmd.wait()
-    assert retcode == 0, "List command returned %d" % retcode
-    for str in list_output.split("\n"):
-        # if (str.startswith(s)):
-        os.system("rosnode kill " + str)
+def calculate_mu(run):
+    if run <= 120:
+        return 0.009
+    elif 120 < run <= 240:
+        return 0.09
+    elif 240 < run <= 360:
+        return 1
+    elif 360 < run <= 480:
+        return 0.05
+    elif 480 < run <= 600:
+        return 0.5
+    elif 600 < run <= 720:
+        return 0.02
+    elif 720 < run <= 840:
+        return 0.2
+    elif 840 < run <= 960:
+        return 0.07
+    elif 960 < run:
+        return 0.7
+    return None
 
 
 def robotUnsafe(robx, roby, path):
@@ -126,7 +98,6 @@ def getLookAheadPoint(waypoints, robx, roby, lookAheadDistance, lastIndex, lastF
         discriminant = sqrt(discriminant)
         t1 = (-b - discriminant) / (2 * a)
         t2 = (-b + discriminant) / (2 * a)
-        # print('t guys', t1, t2)
         if 0 <= t1 <= 1 and j + t1 > lastFractionalIndex:
             return (E[0] + t1 * d[0], E[1] + t1 * d[1]), j, j + t1
         if 0 <= t2 <= 1 and j + t2 > lastFractionalIndex:
@@ -175,10 +146,10 @@ def smoothPath(path):  # path is [(x1, y1), ..., (xend, yend)]
 
 
 def vel_pid(vg, vc, dt):
-    v_goal = vg # output of neural net in m/s
-    v_curr = vc # current velocity in m/s
+    v_goal = vg  # output of neural net in m/s
+    v_curr = vc  # current velocity in m/s
     prev_err = 0.0
-    windup_guard = 10 # needs to be changed??
+    windup_guard = 10  # needs to be changed??
     kp = 1
     ki = 0.1
     kd = 0.1
@@ -190,7 +161,7 @@ def vel_pid(vg, vc, dt):
         i = windup_guard
     elif i > windup_guard:
         i = windup_guard
-    d = 0.0 # use acceleration calculated above??
+    d = 0.0  # use acceleration calculated above??
     if delta_time > 0:
         d = delta_error/dt
     prev_err = error
@@ -198,11 +169,15 @@ def vel_pid(vg, vc, dt):
     return vel
 
 
-# def main(velocity, angle_deg, log_file, run_num):
-def main(angle_deg, run_num):
+def stop_robot(vel_msg, velocity_publisher):
+    vel_msg.linear.x = 0
+    vel_msg.angular.z = 0
+    velocity_publisher.publish(vel_msg)
+
+
+def main(angle_deg):
     velocity_publisher = rospy.Publisher('/jackal_velocity_controller/cmd_vel', Twist, queue_size=10)
     rospy.Subscriber('/gazebo/model_states', ModelStates, statesCallback)
-    info = "{lin_vel}, {ang_vel}, {angle}, {deviation}\n"
     rate = rospy.Rate(10)
     vel_msg = Twist()
     angle = radians(angle_deg)  # in radians
@@ -214,33 +189,26 @@ def main(angle_deg, run_num):
     path = smoothPath(p)
     lookAheadDistance = 2
     lastIndex = 0
-    # lastLookAheadIndex = 0
     lastFractionalIndex = 0
     lookAheadPoint = waypoints[0]
     atGoalHack = 0  # needs to be fixed
-    start_angle = yaw
-    # i = 0
+
     # this is kind of a cop out, we should fix this later
     fut_velocities = [0.3]*len(path)
     pred_vels = [0, 0, 0, 0, 0]
 
-
     while not rospy.is_shutdown():
         path = injectPoints(waypoints2)
         unsafe, robot_deviation, closest_index = robotUnsafe(x, y, path)
         if unsafe:
             print("unsafe")
-            vel_msg.linear.x = 0
-            vel_msg.angular.z = 0
-            velocity_publisher.publish(vel_msg)
+            stop_robot(vel_msg, velocity_publisher)
             print(fut_velocities)
             break
 
         if robotAtGoal(x, y, waypoints[-1][0], waypoints[-1][1]) and lastIndex == len(waypoints) - 1:
             print("at goal:", x, y)
-            vel_msg.linear.x = 0
-            vel_msg.angular.z = 0
-            velocity_publisher.publish(vel_msg)
+            stop_robot(vel_msg, velocity_publisher)
             print(fut_velocities)
             break
 
@@ -260,21 +228,14 @@ def main(angle_deg, run_num):
             except IndexError as e:
                 horizon_point1 = path[-2]
                 horizon_point2 = path[-1]
-            # Estimate angle from starting pose
-            #a = atan2(horizon_point2[1] - horizon_point1[1], horizon_point2[0] - horizon_point1[0]) - start_angle
             # estimate angle from current pose
             a = atan2(horizon_point2[1] - horizon_point1[1], horizon_point2[0] - horizon_point1[0]) - yaw
             ang = abs(degrees(a))
             fut_velocity = model.predict([[mu, ang]])[0][0]
             pred_vels[horizon] = fut_velocity
             horizon = horizon + 1
-        # Current Measure of Safety is slowest but how will that be with more complex systems
+        # current measure of safety is slowest but how will that be with more complex systems
         vel = min(pred_vels)
-        #if (closest_index + horizon) < len(fut_velocities):
-            #fut_velocities[closest_index + horizon] = fut_velocity
-        #test = fut_velocities[closest_index - 1]
-        #print(test)
-        #print(ang)
 
         # linear velocity in the x-axis:
         vel_msg.linear.x = vel
@@ -291,22 +252,11 @@ def main(angle_deg, run_num):
         rate.sleep()
         atGoalHack += 1
 
-        # writing to the log file
-        # log_file.write(info.format(lin_vel=vel_msg.linear.x, ang_vel=vel_msg.angular.z,
-        #                           angle=angle_deg, deviation=robot_deviation))
-
-    # log_file.close()
     print("kill me")
     sys.stdout.flush()
     # time.sleep(20)
     raw_input("")  # kill 0 sent from bash script not working, so you have to ctrl-c manually
 
-    # terminate_process_and_children(proc)
-    # signal_process_and_children(proc.pid, signal.SIGINT, True)
-    # terminate_ros_node()
-    # proc.send_signal(subprocess.signal.SIGINT)
-    # proc.kill()
-    # proc.terminate()
     for process in psutil.process_iter():
         print(process.cmdline())
 
@@ -314,48 +264,12 @@ def main(angle_deg, run_num):
 if __name__ == "__main__":
     rospy.init_node('capstone_nodes', anonymous=True)
 
-    # get run number
     run = 246
     angle = 45
     # run = rospy.get_param('~run')
     # angle = rospy.get_param('~angle')
-    run_num = str(run)
-
-    # automatically calculate angle
-    # automatically calculate mu
-    mu = 0
-    if run <= 120:
-        mu = 0.009
-    elif 120 < run <= 240:
-        mu = 0.09
-    elif 240 < run <= 360:
-        mu = 1
-    elif 360 < run <= 480:
-        mu = 0.05
-    elif 480 < run <= 600:
-        mu = 0.5
-    elif 600 < run <= 720:
-        mu = 0.02
-    elif 720 < run <= 840:
-        mu = 0.2
-    elif 840 < run <= 960:
-        mu = 0.07
-    elif 960 < run:
-        mu = 0.7
-
-    # use neural network to choose velocity
 
+    mu = calculate_mu(run)
     # velocity = model.predict([[mu, angle]])[0][0]
-    # velocity = 1.0
-    env = environments[mu]
-
-    # bag_location = "bagfiles/trainingData" + args.run_num
-
-    # log_file = "../logs/{run}_{env}_{vel}_{angle}.txt".format(run=run_num, env=env, vel=str(velocity), angle=str(angle)) # this needs to be fixed, right now you run test.sh from the bagfiles directory
-    # file format: velocity, angle, path_deviation
-    # file = open(log_file, "w")
 
-    # print("velocity: ", velocity, "angle: ", angle)
-    # print(angle, args.angle)
-    # main(velocity, angle, file, run_num)
-    main(angle, run_num)
+    main(angle)
diff --git a/src/training.py b/src/training.py
index 775c50f..888d179 100755
--- a/src/training.py
+++ b/src/training.py
@@ -1,18 +1,11 @@
 #!/usr/bin/env python
 
-import sys # for redirecting output in bash, could be removed
-#import time # for sleeping - time.sleep is commented out below right now
+import sys
 import os
 import rospy
-import argparse
-import subprocess
 import datetime
-from subprocess import Popen
-import psutil
-import time
 from geometry_msgs.msg import Twist
 import numpy as np
-# import keras
 from nav_msgs.msg import Odometry
 from gazebo_msgs.msg import ModelStates
 from math import pow, atan2, sqrt, ceil, sin, cos, pi, radians
@@ -23,22 +16,12 @@
 v = 0.0
 yaw = 0.0
 
-environments = {0.009: "ice_009", 
-                0.09: "ice_09", 
-                0.9: "ice_9", 
-                1: "control", 
-                1000: "mud",
-                0.05: "ice_05",
-                0.5: "ice_5"}
-
-
 
 def statesCallback(data):
     global x, y, v, yaw
-    # find index of slash
+    # find index of jackal
     name = data.name
     index = name.index("jackal")
-    # index = name.index("/")
     x = data.pose[index].position.x
     y = data.pose[index].position.y
     v = data.twist[index].linear.x
@@ -52,48 +35,52 @@ def statesCallback(data):
     yaw = euler[2]
 
 
-def terminate_process_and_children(p):
-    import psutil
-    process = psutil.Process(p.pid)
-    for sub_process in process.children(recursive=True):
-        sub_process.send_signal(signal.SIGINT)
-    # p.wait()  # we wait for children to terminate
-    p.terminate()
-
-
-def signal_process_and_children(pid, signal_to_send, wait=False):
-    process = psutil.Process(pid)
-    for children in process.children(recursive=True):
-        if signal_to_send == 'suspend':
-            children.suspend()
-        elif signal_to_send == 'resume':
-            children.resume()
-        else:
-            children.send_signal(signal_to_send)
-    if wait:
-        process.wait()
-
-def terminate_ros_node():
-    list_cmd = subprocess.Popen("rosnode list", shell=True, stdout=subprocess.PIPE)
-    list_output = list_cmd.stdout.read()
-    retcode = list_cmd.wait()
-    assert retcode == 0, "List command returned %d" % retcode
-    for str in list_output.split("\n"):
-        #if (str.startswith(s)):
-        os.system("rosnode kill " + str)
-
-
-#def robotUnsafe(robx, roby, path):
+def calculate_mu(run):
+    # 0.2,.5,1,1.5,2,2.5,3,3.5,4,5
+    if run <= 240:
+        return 0.009
+    elif 240 < run <= 480:
+        return 0.09
+    elif 480 < run <= 600:
+        return 1
+    elif 600 < run <= 840:
+        return 0.05
+    elif 840 < run:
+        return 0.5
+    return None
+
+
+def calculate_velocity(run):
+    if run % 10 == 1:
+        return 0.2
+    elif run % 10 == 2:
+        return 0.4
+    elif run % 10 == 3:
+        return 0.6
+    elif run % 10 == 4:
+        return 0.8
+    elif run % 10 == 5:
+        return 1.0
+    elif run % 10 == 6:
+        return 1.2
+    elif run % 10 == 7:
+        return 1.4
+    elif run % 10 == 8:
+        return 1.6
+    elif run % 10 == 9:
+        return 1.8
+    elif run % 10 == 0:
+        return 2.0
+    return None
+
+
 def robotUnsafe(robx, roby, path, safety_tolerance):
     dists = [0]*len(path)
     i = 0
     for point in path:
         dists[i] = sqrt(pow((point[0] - robx), 2) + pow((point[1] - roby), 2))
-        #print(i)
         i = i+1
-    #print(dists)
     val = min(dists)
-    #print(val)
     return val > safety_tolerance, val
 
 
@@ -123,7 +110,6 @@ def getLookAheadPoint(waypoints, robx, roby, lookAheadDistance, lastIndex, lastF
         discriminant = sqrt(discriminant)
         t1 = (-b - discriminant) / (2 * a)
         t2 = (-b + discriminant) / (2 * a)
-        # print('t guys', t1, t2)
         if 0 <= t1 <= 1 and j + t1 > lastFractionalIndex:
             return (E[0] + t1 * d[0], E[1] + t1 * d[1]), j, j + t1
         if 0 <= t2 <= 1 and j + t2 > lastFractionalIndex:
@@ -148,10 +134,8 @@ def injectPoints(waypoints):
         vector = (vector[0] / d * spacing, vector[1] / d * spacing)
         for i in range(0, num_points_that_fit):
             new_list = (start_point[0] + vector[0] * i, start_point[1] + vector[1] * i)
-            #print(new_list)
             new_points.append(new_list)
         new_points.append(end_point)
-    #print(new_points)
     return new_points
 
 
@@ -173,66 +157,56 @@ def smoothPath(path):  # path is [(x1, y1), ..., (xend, yend)]
     return newPath
 
 
-#def main(velocity, angle_deg, run_num):
-def main(velocity, angle_deg, run_num, safety_threshold):
+def stop_robot(vel_msg, velocity_publisher):
+    vel_msg.linear.x = 0
+    vel_msg.angular.z = 0
+    velocity_publisher.publish(vel_msg)
+
+
+def main(velocity, angle_deg, safety_threshold):
     velocity_publisher = rospy.Publisher('/jackal_velocity_controller/cmd_vel', Twist, queue_size=10)
     rospy.Subscriber('/gazebo/model_states', ModelStates, statesCallback)
-    info = "{lin_vel}, {ang_vel}, {angle}, {deviation}\n"
     rate = rospy.Rate(10)
     vel_msg = Twist()
     angle = radians(angle_deg)  # in radians
     branching_point = (10, 0)
     end_point = (branching_point[0] + 10*cos(angle), 10*sin(angle))
     print(end_point)
-    # waypoints = [(10, 0), (0, 10), (10, 10), (0, 0)]
     waypoints = [branching_point, end_point]
     waypoints2 = [(0, 0), branching_point, end_point]
     path = injectPoints(waypoints2)
-    path = smoothPath(path)
     lookAheadDistance = 2
     lastIndex = 0
-    # lastLookAheadIndex = 0
     lastFractionalIndex = 0
     lookAheadPoint = waypoints[0]
     atGoalHack = 0  # needs to be fixed
-    # i = 0
 
     begin = datetime.datetime.now()
     time_to_stop = 4  # in minutes
- 
 
     while not rospy.is_shutdown():
         path = injectPoints(waypoints2)
-        #unsafe, robot_deviation = robotUnsafe(x, y, path)
         unsafe, robot_deviation = robotUnsafe(x, y, path, safety_threshold)
         if unsafe:
             print("unsafe")
-            vel_msg.linear.x = 0
-            vel_msg.angular.z = 0
-            velocity_publisher.publish(vel_msg)
+            stop_robot(vel_msg, velocity_publisher)
             break
 
         if robotAtGoal(x, y, waypoints[-1][0], waypoints[-1][1]) and lastIndex == len(waypoints) - 1:
-        #if robotAtGoal(x, y, waypoints[-1][0], waypoints[-1][1]) and lastIndex == len(waypoints) - 1 and atGoalHack>100:
             print("at goal:", x, y)
-            vel_msg.linear.x = 0
-            vel_msg.angular.z = 0
-            velocity_publisher.publish(vel_msg)
+            stop_robot(vel_msg, velocity_publisher)
+            break
+
+        now = datetime.datetime.now()
+        # will stop program if robot hasn't found goal or become unsafe after time_to_stop minutes
+        if begin + datetime.timedelta(minutes=time_to_stop) < now:
+            print("timed out")
+            stop_robot(vel_msg, velocity_publisher)
             break
 
         lookAheadPoint, lastIndex, lastFractionalIndex = getLookAheadPoint(waypoints, x, y, lookAheadDistance,
                                                                            lastIndex, lastFractionalIndex,
                                                                            lookAheadPoint)
-        
-        # will stop program if robot hasn't found goal or become unsafe after 4 minutes       
-        now = datetime.datetime.now()        
-        if begin + datetime.timedelta(minutes = time_to_stop) < now:
-            print("timed out")
-            vel_msg.linear.x = 0
-            vel_msg.angular.z = 0
-            velocity_publisher.publish(vel_msg)
-            break
-            
 
         goal_pose_x = lookAheadPoint[0]
         goal_pose_y = lookAheadPoint[1]
@@ -263,65 +237,14 @@ def main(velocity, angle_deg, run_num, safety_threshold):
 
 if __name__ == "__main__":
     rospy.init_node('capstone_nodes', anonymous=True)
-    
-    # get run number
+
     run = rospy.get_param('~run')
     angle = rospy.get_param('~angle')
     safety_threshold = rospy.get_param('~safety_threshold')
-    run_num=str(run)
-    
-    # automatically calculate velocity
-    if run % 10 == 1:
-        velocity = 0.2
-    elif run % 10 == 2:
-    	velocity = 0.4
-    elif run % 10 == 3:
-        velocity = 0.6
-    elif run % 10 == 4:
-        velocity = 0.8
-    elif run % 10 == 5:
-        velocity = 1.0
-    elif run % 10 == 6:
-    	velocity = 1.2
-    elif run % 10 == 7:
-        velocity = 1.4
-    elif run % 10 == 8:
-        velocity = 1.6
-    elif run % 10 == 9:
-        velocity = 1.8
-    elif run % 10 == 0:
-        velocity = 2.0
-        
-    # automatically calculate angle
-    # automatically calculate mu
-    	'''
-    mu = 0
-    if run <= 1200: #0.2,.5,1,1.5,2,2.5,3,3.5,4,5
-        mu = 0.009
-    elif 1200 < run <= 2400:
-        mu = 0.09
-    elif 2400 < run <= 4800:
-        mu = 1
-    elif 4800 < run <= 6000:
-        mu = 0.05
-    elif 6000 < run:
-        mu = 0.5
-    '''
-    mu = 0
-    if run <= 240: #0.2,.5,1,1.5,2,2.5,3,3.5,4,5
-        mu = 0.009
-    elif 240 < run <= 480:
-        mu = 0.09
-    elif 480 < run <= 600:
-        mu = 1
-    elif 600 < run <= 840:
-        mu = 0.05
-    elif 840 < run:
-        mu = 0.5
-    env = environments[mu]
+    mu = calculate_mu(run)
+    velocity = calculate_velocity(run)
 
     print("velocity: ", velocity, "angle: ", angle)
-    #main(velocity, angle, run_num)
-    main(velocity, angle, run_num, safety_threshold)
+    main(velocity, angle, safety_threshold)
 
 
diff --git a/src/workplease.py b/src/workplease.py
index d78e8df..7ecbe84 100755
--- a/src/workplease.py
+++ b/src/workplease.py
@@ -1,14 +1,8 @@
 #!/usr/bin/env python
 
-import sys  # for redirecting output in bash, could be removed
-# import time # for sleeping - time.sleep is commented out below right now
-import os
+import sys
 import rospy
-import argparse
-import subprocess
-from subprocess import Popen
 import psutil
-import time
 from geometry_msgs.msg import Twist
 import numpy as np
 import keras
@@ -22,28 +16,15 @@
 v = 0.0
 yaw = 0.0
 
-environments = {0.009: "ice_009",
-                0.09: "ice_09",
-                0.9: "ice_9",
-                1: "control",
-                1000: "mud",
-                0.05: "ice_05",
-                0.5: "ice_5",
-                0.02: "ice_02",
-                0.2: "ice_2",
-                0.07: "ice_07",
-                0.7: "ice_7"}
-
 model = keras.models.load_model('/home/bezzo/catkin_ws/src/capstone_nodes/NNet_all_tf_210.h5', custom_objects={
     'Normalization': keras.layers.experimental.preprocessing.Normalization()})
 
 
 def statesCallback(data):
     global x, y, v, yaw
-    # find index of slash
+    # find index of jackal
     name = data.name
     index = name.index("jackal")
-    # index = name.index("/")
     x = data.pose[index].position.x
     y = data.pose[index].position.y
     v = data.twist[index].linear.x
@@ -57,36 +38,26 @@ def statesCallback(data):
     yaw = euler[2]
 
 
-def terminate_process_and_children(p):
-    import psutil
-    process = psutil.Process(p.pid)
-    for sub_process in process.children(recursive=True):
-        sub_process.send_signal(signal.SIGINT)
-    # p.wait()  # we wait for children to terminate
-    p.terminate()
-
-
-def signal_process_and_children(pid, signal_to_send, wait=False):
-    process = psutil.Process(pid)
-    for children in process.children(recursive=True):
-        if signal_to_send == 'suspend':
-            children.suspend()
-        elif signal_to_send == 'resume':
-            children.resume()
-        else:
-            children.send_signal(signal_to_send)
-    if wait:
-        process.wait()
-
-
-def terminate_ros_node():
-    list_cmd = subprocess.Popen("rosnode list", shell=True, stdout=subprocess.PIPE)
-    list_output = list_cmd.stdout.read()
-    retcode = list_cmd.wait()
-    assert retcode == 0, "List command returned %d" % retcode
-    for str in list_output.split("\n"):
-        # if (str.startswith(s)):
-        os.system("rosnode kill " + str)
+def calculate_mu(run):
+    if run <= 120:
+        return 0.009
+    elif 120 < run <= 240:
+        return 0.09
+    elif 240 < run <= 360:
+        return 1
+    elif 360 < run <= 480:
+        return 0.05
+    elif 480 < run <= 600:
+        return 0.5
+    elif 600 < run <= 720:
+        return 0.02
+    elif 720 < run <= 840:
+        return 0.2
+    elif 840 < run <= 960:
+        return 0.07
+    elif 960 < run:
+        return 0.7
+    return None
 
 
 def robotUnsafe(robx, roby, path, tol):
@@ -127,7 +98,7 @@ def getLookAheadPoint(waypoints, robx, roby, lookAheadDistance, lastIndex, lastF
         discriminant = sqrt(discriminant)
         t1 = (-b - discriminant) / (2 * a)
         t2 = (-b + discriminant) / (2 * a)
-        # print('t guys', t1, t2)
+
         if 0 <= t1 <= 1 and j + t1 > lastFractionalIndex:
             return (E[0] + t1 * d[0], E[1] + t1 * d[1]), j, j + t1
         if 0 <= t2 <= 1 and j + t2 > lastFractionalIndex:
@@ -176,10 +147,10 @@ def smoothPath(path):  # path is [(x1, y1), ..., (xend, yend)]
 
 
 def vel_pid(vg, vc, dt):
-    v_goal = vg # output of neural net in m/s
-    v_curr = vc # current velocity in m/s
+    v_goal = vg  # output of neural net in m/s
+    v_curr = vc  # current velocity in m/s
     prev_err = 0.0
-    windup_guard = 10 # needs to be changed??
+    windup_guard = 10  # needs to be changed??
     kp = 1
     ki = 0.1
     kd = 0.1
@@ -191,7 +162,7 @@ def vel_pid(vg, vc, dt):
         i = windup_guard
     elif i > windup_guard:
         i = windup_guard
-    d = 0.0 # use acceleration calculated above??
+    d = 0.0  # use acceleration calculated above??
     if delta_time > 0:
         d = delta_error/dt
     prev_err = error
@@ -199,11 +170,15 @@ def vel_pid(vg, vc, dt):
     return vel
 
 
-# def main(velocity, angle_deg, log_file, run_num, safety_tol):
-def main(angle_deg, run_num, safety_tol):
+def stop_robot(vel_msg, velocity_publisher):
+    vel_msg.linear.x = 0
+    vel_msg.angular.z = 0
+    velocity_publisher.publish(vel_msg)
+
+
+def main(angle_deg, safety_tol):
     velocity_publisher = rospy.Publisher('/jackal_velocity_controller/cmd_vel', Twist, queue_size=10)
     rospy.Subscriber('/gazebo/model_states', ModelStates, statesCallback)
-    info = "{lin_vel}, {ang_vel}, {angle}, {deviation}\n"
     rate = rospy.Rate(10)
     vel_msg = Twist()
     angle = radians(angle_deg)  # in radians
@@ -212,35 +187,24 @@ def main(angle_deg, run_num, safety_tol):
     waypoints = [branching_point, end_point]
     waypoints2 = [(0, 0), branching_point, end_point]
     p = injectPoints(waypoints2)
-    path = smoothPath(p)
     lookAheadDistance = 2
     lastIndex = 0
-    # lastLookAheadIndex = 0
     lastFractionalIndex = 0
     lookAheadPoint = waypoints[0]
     atGoalHack = 0  # needs to be fixed
-    start_angle = yaw
-    # i = 0
-    # this is kind of a cop out, we should fix this later
-    fut_velocities = [0.3]*len(path)
     pred_vels = [0]*5
 
-
     while not rospy.is_shutdown():
         path = injectPoints(waypoints2)
         unsafe, robot_deviation, closest_index = robotUnsafe(x, y, path, safety_tol)
         if unsafe:
             print("unsafe")
-            vel_msg.linear.x = 0
-            vel_msg.angular.z = 0
-            velocity_publisher.publish(vel_msg)
+            stop_robot(vel_msg, velocity_publisher)
             break
 
         if robotAtGoal(x, y, waypoints[-1][0], waypoints[-1][1]) and lastIndex == len(waypoints) - 1:
             print("at goal:", x, y)
-            vel_msg.linear.x = 0
-            vel_msg.angular.z = 0
-            velocity_publisher.publish(vel_msg)
+            stop_robot(vel_msg, velocity_publisher)
             break
 
         lookAheadPoint, lastIndex, lastFractionalIndex = getLookAheadPoint(waypoints, x, y, lookAheadDistance,
@@ -259,22 +223,15 @@ def main(angle_deg, run_num, safety_tol):
             except IndexError as e:
                 horizon_point1 = path[-2]
                 horizon_point2 = path[-1]
-            # Estimate angle from starting pose
-            #a = atan2(horizon_point2[1] - horizon_point1[1], horizon_point2[0] - horizon_point1[0]) - start_angle
             # estimate angle from current pose
             a = atan2(horizon_point2[1] - horizon_point1[1], horizon_point2[0] - horizon_point1[0]) - yaw
             ang = abs(degrees(a))
             fut_velocity = model.predict([[mu, ang, safety_tol]])[0][0]
             pred_vels[horizon] = fut_velocity
             horizon = horizon + 1
-        # Current Measure of Safety is slowest but how will that be with more complex systems
+        # current measure of safety is slowest but how will that be with more complex systems
         vel = min(pred_vels)
         print(vel)
-        #if (closest_index + horizon) < len(fut_velocities):
-            #fut_velocities[closest_index + horizon] = fut_velocity
-        #test = fut_velocities[closest_index - 1]
-        #print(test)
-        #print(ang)
 
         # linear velocity in the x-axis:
         vel_msg.linear.x = vel
@@ -291,22 +248,11 @@ def main(angle_deg, run_num, safety_tol):
         rate.sleep()
         atGoalHack += 1
 
-        # writing to the log file
-        # log_file.write(info.format(lin_vel=vel_msg.linear.x, ang_vel=vel_msg.angular.z,
-        #                           angle=angle_deg, deviation=robot_deviation))
-
-    # log_file.close()
     print("kill me")
     sys.stdout.flush()
     # time.sleep(20)
     raw_input("")  # kill 0 sent from bash script not working, so you have to ctrl-c manually
 
-    # terminate_process_and_children(proc)
-    # signal_process_and_children(proc.pid, signal.SIGINT, True)
-    # terminate_ros_node()
-    # proc.send_signal(subprocess.signal.SIGINT)
-    # proc.kill()
-    # proc.terminate()
     for process in psutil.process_iter():
         print(process.cmdline())
 
@@ -314,49 +260,12 @@ def main(angle_deg, run_num, safety_tol):
 if __name__ == "__main__":
     rospy.init_node('capstone_nodes', anonymous=True)
 
-    # get run number
     run = 12
     angle = 45
+    mu = calculate_mu(run)
+    safety_tol = 2
     # run = rospy.get_param('~run')
     # angle = rospy.get_param('~angle')
-    run_num = str(run)
-    safety_tol = 2
-
-    # automatically calculate angle
-    # automatically calculate mu
-    mu = 0
-    if run <= 120:
-        mu = 0.009
-    elif 120 < run <= 240:
-        mu = 0.09
-    elif 240 < run <= 360:
-        mu = 1
-    elif 360 < run <= 480:
-        mu = 0.05
-    elif 480 < run <= 600:
-        mu = 0.5
-    elif 600 < run <= 720:
-        mu = 0.02
-    elif 720 < run <= 840:
-        mu = 0.2
-    elif 840 < run <= 960:
-        mu = 0.07
-    elif 960 < run:
-        mu = 0.7
-
-    # use neural network to choose velocity
-
     # velocity = model.predict([[mu, angle]])[0][0]
-    # velocity = 1.0
-    env = environments[mu]
-
-    # bag_location = "bagfiles/trainingData" + args.run_num
-
-    # log_file = "../logs/{run}_{env}_{vel}_{angle}.txt".format(run=run_num, env=env, vel=str(velocity), angle=str(angle)) # this needs to be fixed, right now you run test.sh from the bagfiles directory
-    # file format: velocity, angle, path_deviation
-    # file = open(log_file, "w")
 
-    # print("velocity: ", velocity, "angle: ", angle)
-    # print(angle, args.angle)
-    # main(velocity, angle, file, run_num)
-    main(angle, run_num, safety_tol)
+    main(angle, safety_tol)