diff --git a/.moves.py.swp b/.moves.py.swp
new file mode 100644
index 0000000..310a83d
Binary files /dev/null and b/.moves.py.swp differ
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..3314378
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,199 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(final_project)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  geometry_msgs
+  rospy
+  smach
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   geometry_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES final_project
+#  CATKIN_DEPENDS geometry_msgs rospy smach
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/final_project.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/final_project_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_final_project.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/ChaChaDance/__init__.py b/ChaChaDance/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/ChaChaDance/__init__.pyc b/ChaChaDance/__init__.pyc
new file mode 100644
index 0000000..9b885a0
Binary files /dev/null and b/ChaChaDance/__init__.pyc differ
diff --git a/ChaChaDance/backward.py b/ChaChaDance/backward.py
new file mode 100644
index 0000000..0724057
--- /dev/null
+++ b/ChaChaDance/backward.py
@@ -0,0 +1,38 @@
+#!/usr/bin/env python
+
+# Import libraries
+import rospy
+import smach
+import time
+from geometry_msgs.msg import Twist
+
+
+# Define backward state
+class Backward(smach.State):
+    def __init__(self, pub_controls):
+        smach.State.__init__(self, outcomes=["do_plan"], input_keys=["curr_state"])
+        self.counter = 0
+        self.pub_controls = pub_controls
+
+    def execute(self, userdata):
+        # get state attributes
+        state_name = userdata.curr_state["name"]
+
+        rospy.loginfo("Running {} state".format(state_name))
+
+        dur = rospy.Duration(2.8 / (6.0))
+        rate = rospy.Rate(10)
+        start = rospy.Time.now()
+
+        while rospy.Time.now() - start < dur:
+            move_cmd = Twist()
+            move_cmd.linear.x = -3.0
+            self.pub_controls.publish(move_cmd)
+            rate.sleep()
+
+        move_cmd = Twist()
+        move_cmd.linear.x = 0.0
+        self.pub_controls.publish(move_cmd)
+
+        return "do_plan"
+
diff --git a/ChaChaDance/backward.pyc b/ChaChaDance/backward.pyc
new file mode 100644
index 0000000..2f146da
Binary files /dev/null and b/ChaChaDance/backward.pyc differ
diff --git a/ChaChaDance/circle.py b/ChaChaDance/circle.py
new file mode 100644
index 0000000..b0e7ba4
--- /dev/null
+++ b/ChaChaDance/circle.py
@@ -0,0 +1,44 @@
+#!/usr/bin/env python
+
+# Import libraries
+import rospy
+import smach
+import time
+import math
+from geometry_msgs.msg import Twist
+
+
+# Define circle state
+class Circle(smach.State):
+    def __init__(self, pub_controls):
+        smach.State.__init__(self, outcomes=["do_plan"], input_keys=["curr_state"])
+        self.counter = 0
+        self.pub_controls = pub_controls
+
+    def execute(self, userdata):
+        # get state attributes
+        state_name = userdata.curr_state["name"]
+        direction = userdata.curr_state["direction"]
+
+        rospy.loginfo("Running {} state".format(state_name))
+
+        dur = rospy.Duration(47 / (70.0))
+        rate = rospy.Rate(10)
+
+        move_cmd = Twist()
+        move_cmd.linear.x= 5
+        move_cmd.angular.z = 5 * direction
+
+        t0=rospy.Time.now()
+        while rospy.Time.now() - t0 < dur:
+            self.pub_controls.publish(move_cmd)
+            rate.sleep()
+
+        #After the loop, stops the robot
+        move_cmd = Twist()
+        move_cmd.angular.z = 0
+        move_cmd.linear.x = 0
+        self.pub_controls.publish(move_cmd)
+
+        return "do_plan"
+    
diff --git a/ChaChaDance/circle.pyc b/ChaChaDance/circle.pyc
new file mode 100644
index 0000000..cf234fd
Binary files /dev/null and b/ChaChaDance/circle.pyc differ
diff --git a/ChaChaDance/forward.py b/ChaChaDance/forward.py
new file mode 100644
index 0000000..af67d04
--- /dev/null
+++ b/ChaChaDance/forward.py
@@ -0,0 +1,38 @@
+#!/usr/bin/env python
+
+# Import libraries
+import rospy
+import smach
+import time
+from geometry_msgs.msg import Twist
+
+
+# Define forward state
+class Forward(smach.State):
+    def __init__(self, pub_controls):
+        smach.State.__init__(self, outcomes=["do_plan"], input_keys=["curr_state"])
+        self.counter = 0
+        self.pub_controls = pub_controls
+
+    def execute(self, userdata):
+        # get state attributes
+        state_name = userdata.curr_state["name"]
+
+        rospy.loginfo("Running {} state".format(state_name))
+
+        dur = rospy.Duration(2.8 / (6.0))
+        rate = rospy.Rate(10)
+        start = rospy.Time.now()
+
+        while rospy.Time.now() - start < dur:
+            move_cmd = Twist()
+            move_cmd.linear.x = 3.0
+            self.pub_controls.publish(move_cmd)
+            rate.sleep()
+
+        move_cmd = Twist()
+        move_cmd.linear.x = 0.0
+        self.pub_controls.publish(move_cmd)
+
+        return "do_plan"
+
diff --git a/ChaChaDance/forward.pyc b/ChaChaDance/forward.pyc
new file mode 100644
index 0000000..eb4f6e1
Binary files /dev/null and b/ChaChaDance/forward.pyc differ
diff --git a/ChaChaDance/plan.py b/ChaChaDance/plan.py
new file mode 100644
index 0000000..dd06a75
--- /dev/null
+++ b/ChaChaDance/plan.py
@@ -0,0 +1,31 @@
+#!/usr/bin/env python
+
+# Import libraries
+import rospy
+import smach
+import time
+
+
+# Define planning state
+class Plan(smach.State):
+    def __init__(self):
+        smach.State.__init__(self, outcomes=["do_forward", "do_backward", "do_exit",
+                                             "do_turnL", "do_turnR", "do_circle"],
+                            input_keys=["plan", "curr_state", "plan_length"],
+                            output_keys=["curr_state"])
+
+        self.counter = 0
+
+    def execute(self, userdata):
+        # if plan is empty, exit
+        if not userdata.plan:
+            return 'do_exit'
+
+        # get the first state
+        current_state = userdata.plan.pop(0)
+        userdata.curr_state = current_state
+        rospy.loginfo("Planning attempting to run: {}".format(current_state["name"]))
+
+        time.sleep(1)
+
+        return current_state["name"]
diff --git a/ChaChaDance/plan.pyc b/ChaChaDance/plan.pyc
new file mode 100644
index 0000000..4d43320
Binary files /dev/null and b/ChaChaDance/plan.pyc differ
diff --git a/ChaChaDance/stop.py b/ChaChaDance/stop.py
new file mode 100644
index 0000000..a716ce5
--- /dev/null
+++ b/ChaChaDance/stop.py
@@ -0,0 +1,26 @@
+#!/usr/bin/env python
+
+# Import libraries
+import rospy
+import smach
+import time
+from geometry_msgs.msg import Twist
+
+
+# Define the stop state
+class Stop(smach.State):
+    def __init__(self,  pub_controls):
+        smach.State.__init__(self, outcomes=["do_exit"])
+        self.counter = 0
+        self.pub_controls = pub_controls
+
+    def execute(self, userdata):
+        rospy.loginfo("Running Stop state")
+
+        move_cmd = Twist()
+        move_cmd.linear.x = 0.0
+        self.pub_controls.publish(move_cmd)
+
+        time.sleep(1)
+
+        return "do_exit"
diff --git a/ChaChaDance/stop.pyc b/ChaChaDance/stop.pyc
new file mode 100644
index 0000000..600f38b
Binary files /dev/null and b/ChaChaDance/stop.pyc differ
diff --git a/ChaChaDance/turnL.py b/ChaChaDance/turnL.py
new file mode 100644
index 0000000..8758e1a
--- /dev/null
+++ b/ChaChaDance/turnL.py
@@ -0,0 +1,40 @@
+#!/usr/bin/env python
+
+# Import libraries
+import rospy
+import smach
+import time
+from geometry_msgs.msg import Twist
+
+
+# define state turnL - turn left
+class TurnL(smach.State):
+    def __init__(self, pub_controls):
+        smach.State.__init__(self, outcomes=['do_plan'],
+                            input_keys=['curr_state'])
+        self.pub_controls = pub_controls
+        self.counter = 0
+
+    def execute(self, userdata):
+        # get state attributes
+        state_name = userdata.curr_state["name"]
+
+        rospy.loginfo("Running {} state".format(state_name))
+
+        move_cmd = Twist()
+        move_cmd.angular.z = 10
+        
+        rate = rospy.Rate(10)
+        
+        dur = rospy.Duration(3.5 / (7.0))
+        t0 = rospy.Time.now()
+
+        while rospy.Time.now() - t0 < dur:
+            self.pub_controls.publish(move_cmd)
+            rate.sleep()
+
+        move_cmd = Twist()
+        move_cmd.angular.z = 0.0
+        self.pub_controls.publish(move_cmd)
+
+        return "do_plan"
diff --git a/ChaChaDance/turnL.pyc b/ChaChaDance/turnL.pyc
new file mode 100644
index 0000000..3f4f7f4
Binary files /dev/null and b/ChaChaDance/turnL.pyc differ
diff --git a/ChaChaDance/turnR.py b/ChaChaDance/turnR.py
new file mode 100644
index 0000000..7ef6e78
--- /dev/null
+++ b/ChaChaDance/turnR.py
@@ -0,0 +1,40 @@
+#!/usr/bin/env python
+
+# Import libraries
+import rospy
+import smach
+import time
+from geometry_msgs.msg import Twist
+
+
+# define state turnR - turn right
+class TurnR(smach.State):
+    def __init__(self, pub_controls):
+        smach.State.__init__(self, outcomes=['do_plan'],
+                            input_keys=['curr_state'])
+        self.pub_controls = pub_controls
+        self.counter = 0
+
+    def execute(self, userdata):
+        # get state attributes
+        state_name = userdata.curr_state["name"]
+
+        rospy.loginfo("Running {} state".format(state_name))
+
+        move_cmd = Twist()
+        move_cmd.angular.z = -10
+        
+        rate = rospy.Rate(10)
+        
+        dur = rospy.Duration(3.5 / (7.0))
+        t0 = rospy.Time.now()
+
+        while rospy.Time.now() - t0 < dur:
+            self.pub_controls.publish(move_cmd)
+            rate.sleep()
+
+        move_cmd = Twist()
+        move_cmd.angular.z = 0.0
+        self.pub_controls.publish(move_cmd)
+
+        return "do_plan"
diff --git a/ChaChaDance/turnR.pyc b/ChaChaDance/turnR.pyc
new file mode 100644
index 0000000..d8620c4
Binary files /dev/null and b/ChaChaDance/turnR.pyc differ
diff --git a/FinalProject.launch b/FinalProject.launch
new file mode 100755
index 0000000..8ac6139
--- /dev/null
+++ b/FinalProject.launch
@@ -0,0 +1,4 @@
+<launch>
+    <node pkg="final_project" type="main.py" name="final_project" output="screen">
+    </node>
+</launch>
diff --git a/__pycache__/plan_handler.cpython-35.pyc b/__pycache__/plan_handler.cpython-35.pyc
new file mode 100644
index 0000000..3e16837
Binary files /dev/null and b/__pycache__/plan_handler.cpython-35.pyc differ
diff --git a/__pycache__/plan_handler_test.cpython-27-PYTEST.pyc b/__pycache__/plan_handler_test.cpython-27-PYTEST.pyc
new file mode 100644
index 0000000..ba51ded
Binary files /dev/null and b/__pycache__/plan_handler_test.cpython-27-PYTEST.pyc differ
diff --git a/main.py b/main.py
new file mode 100755
index 0000000..3d47238
--- /dev/null
+++ b/main.py
@@ -0,0 +1,95 @@
+#!/usr/bin/env python
+
+# Import libraries
+import rospy
+import smach
+import time
+import pathlib
+import websocket
+import thread
+import time
+from plan_handler import get_filtered_plans
+from geometry_msgs.msg import Twist
+from plan_parser import parse_plan
+from ChaChaDance.plan import Plan
+from ChaChaDance.stop import Stop
+from ChaChaDance.forward import Forward
+from ChaChaDance.backward import Backward
+from ChaChaDance.turnL import TurnL
+from ChaChaDance.turnR import TurnR
+from ChaChaDance.circle import Circle
+
+def on_message(ws, message):
+    plan = get_filtered_plans(message)
+    if plan:
+        runTurtleBot(plan)
+
+def on_error(ws, error):
+    print(error)
+
+
+def on_close(ws, close_status_code, close_msg):
+    print("### closed ###")
+
+
+def on_open(ws):
+    def run(*args):
+        time.sleep(1)
+        print("WSS connection opened")
+    thread.start_new_thread(run, ())
+
+
+def runTurtleBot(plan):
+    pub_controls = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
+
+    # Create a SMACH state machine
+    sm = smach.StateMachine(outcomes=['do_exit'])
+    sm.userdata.plan = plan
+    sm.userdata.plan_length = len(plan)
+    sm.userdata.curr_state = {}
+
+    with sm:
+        # Add states to the container
+        smach.StateMachine.add('Plan', Plan(),
+                                transitions={'do_forward': 'Forward', 'do_backward': 'Backward',
+                                             'do_turnL': 'TurnL', 'do_turnR': 'TurnR', 'do_exit': 'Stop',
+                                             'do_circle': 'Circle'})
+        smach.StateMachine.add('Forward', Forward(pub_controls),
+                                transitions={'do_plan': 'Plan'},
+                                remapping={"curr_state": "curr_state"})
+        smach.StateMachine.add("Backward", Backward(pub_controls),
+                                transitions={"do_plan": "Plan"},
+                                remapping={"curr_state": "curr_state"})
+        smach.StateMachine.add("TurnL", TurnL(pub_controls),
+                                transitions={"do_plan": "Plan"},
+                                remapping={"curr_state": "curr_state"})
+        smach.StateMachine.add("TurnR", TurnR(pub_controls),
+                                transitions={"do_plan": "Plan"},
+                                remapping={"curr_state": "curr_state"})
+        smach.StateMachine.add("Circle", Circle(pub_controls),
+                                transitions={"do_plan": "Plan"},
+                                remapping={"curr_state": "curr_state"})
+        smach.StateMachine.add("Stop", Stop(pub_controls),
+                                transitions={"do_exit": "do_exit"})
+        outcome = sm.execute()
+
+
+def read_cha_cha():
+    websocket.enableTrace(True)
+    ws = websocket.WebSocketApp("wss://squushy.herokuapp.com",
+                                on_open=on_open,
+                                on_message=on_message,
+                                on_error=on_error,
+                                on_close=on_close)
+    ws.run_forever()
+
+
+if __name__ == '__main__':
+    rospy.init_node('squishy_turtlebot3')
+
+    abs_path = pathlib.Path(__file__).parent.resolve()
+    plan_path = str(abs_path) + "/plans/plan1.json"
+    plan = parse_plan(plan_path)
+    print(plan)
+
+    read_cha_cha()
diff --git a/moves.py b/moves.py
new file mode 100644
index 0000000..4cdbeaa
--- /dev/null
+++ b/moves.py
@@ -0,0 +1,84 @@
+#!/usr/bin/env python
+
+# Import libraries
+import rospy
+import smach
+import time
+import math
+from geometry_msgs.msg import Twist
+
+def move_circle(pub_controls, direction):
+    rospy.loginfo("Running circle state. Direction: {}".format(direction))
+
+    dur = rospy.Duration(47 / (20.0))
+    rate = rospy.Rate(10)
+
+    move_cmd = Twist()
+    move_cmd.linear.x= 5
+    move_cmd.angular.z = 5 * direction
+
+    t0=rospy.Time.now()
+    while rospy.Time.now() - t0 < dur:
+        pub_controls.publish(move_cmd)
+        rate.sleep()
+
+    #After the loop, stops the robot
+    move_cmd = Twist()
+    move_cmd.angular.z = 0
+    move_cmd.linear.x = 0
+    pub_controls.publish(move_cmd)
+
+def move_forward(pub_controls, direction):
+    rospy.loginfo("Running forward state. Direction: {}".format(direction))
+
+    dur = rospy.Duration(10.8 / (6.0))
+    rate = rospy.Rate(10)
+    start = rospy.Time.now()
+
+    while rospy.Time.now() - start < dur:
+        move_cmd = Twist()
+        move_cmd.linear.x = 3.0 * direction
+        pub_controls.publish(move_cmd)
+        rate.sleep()
+
+    move_cmd = Twist()
+    move_cmd.linear.x = 0.0
+    pub_controls.publish(move_cmd)
+
+def move_left(pub_controls):
+    rospy.loginfo("Running left state.")
+
+    move_cmd = Twist()
+    move_cmd.angular.z = 10
+
+    rate = rospy.Rate(10)
+
+    dur = rospy.Duration(3.5 / (7.0))
+    t0 = rospy.Time.now()
+
+    while rospy.Time.now() - t0 < dur:
+        pub_controls.publish(move_cmd)
+        rate.sleep()
+
+    move_cmd = Twist()
+    move_cmd.angular.z = 0.0
+    pub_controls.publish(move_cmd)
+
+def move_right(pub_controls):
+    rospy.loginfo("Running left state.")
+
+    move_cmd = Twist()
+    move_cmd.angular.z = -10
+
+    rate = rospy.Rate(10)
+
+    dur = rospy.Duration(3.5 / (7.0))
+    t0 = rospy.Time.now()
+
+    while rospy.Time.now() - t0 < dur:
+        pub_controls.publish(move_cmd)
+        rate.sleep()
+
+    move_cmd = Twist()
+    move_cmd.angular.z = 0.0
+    pub_controls.publish(move_cmd)
diff --git a/moves.pyc b/moves.pyc
new file mode 100644
index 0000000..f5b2623
Binary files /dev/null and b/moves.pyc differ
diff --git a/package.xml b/package.xml
new file mode 100644
index 0000000..37751af
--- /dev/null
+++ b/package.xml
@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>final_project</name>
+  <version>0.0.0</version>
+  <description>The final_project package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="pi@todo.todo">pi</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/final_project</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>smach</build_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>smach</build_export_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>smach</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
diff --git a/plan_handler.py b/plan_handler.py
new file mode 100644
index 0000000..e9c9f56
--- /dev/null
+++ b/plan_handler.py
@@ -0,0 +1,264 @@
+#!/usr/bin/env/ python
+
+# Import libraries
+import rospy
+from geometry_msgs.msg import Twist
+from moves import move_circle, move_forward, move_left, move_right
+from datetime import timedelta, datetime
+
+# Global Counters
+in_progress = False
+last_moves = []
+last_move_time = datetime.utcnow() - timedelta(days=1)
+
+# Return plans depending on song filter
+def get_filtered_plans(last_words):
+    last_words = ' '.join(last_words.split()[-5:])
+
+    global in_progress
+    global last_move_time
+
+    if in_progress:
+        return []
+
+    in_progress = True
+
+    last_words_str = str(last_words)
+    last_words_lower = last_words_str.lower()
+
+    #print(last_words_lower)
+    cur_time = datetime.utcnow()
+
+    pub_controls = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
+    if cur_time > last_move_time + timedelta(seconds=2.5):
+        cha_cha_handler(last_words_lower, pub_controls)
+        clap_handler(last_words_lower, pub_controls)
+        left_handler(last_words_lower, pub_controls)
+        back_handler(last_words_lower, pub_controls)
+        one_hop_handler(last_words_lower, pub_controls)
+        right_foot_handler(last_words_lower, pub_controls)
+        left_foot_handler(last_words_lower, pub_controls)
+        circle_handler(last_words_lower, pub_controls)
+
+    in_progress = False
+
+    return []
+
+
+def cha_cha_handler(last_words, pub_controls):
+    global last_moves
+    global last_move_time
+
+    if len(last_moves) >= 1 and \
+            last_moves[-1] == 'cha cha':
+                return []
+
+    matches = [
+        "we're gonna get funky",
+        "we're going to get funky",
+        "cha-cha",
+        "cha cha",
+        "time to get funky"
+    ]
+
+    for match in matches:
+        if match in last_words:
+            last_move_time = datetime.utcnow()
+            last_moves.append('cha cha')
+
+            print("Found cha-cha move. Attempting to execute.")
+
+            move_circle(pub_controls, 1)
+            move_circle(pub_controls, -1)
+
+            return True
+
+    return False
+
+
+def clap_handler(last_words, pub_controls):
+    global last_moves
+    global last_move_time
+
+    if len(last_moves) >= 2 and \
+            last_moves[-1] == 'clap':
+                return []
+
+    matches = [
+        "everybody clap your hands",
+        "clap clap clap"
+    ]
+
+    for match in matches:
+        if match in last_words:
+            last_move_time = datetime.utcnow()
+            last_moves.append('clap')
+
+            print("Found clap. Attempting to execute.")
+
+            move_forward(pub_controls, 1)
+            move_forward(pub_controls, -1)
+
+            return True
+
+    return False
+
+
+def left_handler(last_words, pub_controls):
+    global last_moves
+    global last_move_time
+
+    if len(last_moves) >= 1 and \
+            last_moves[-1] == 'left':
+                return []
+
+    matches = [
+        "to the left",
+        "turn the lamp"
+    ]
+
+    for match in matches:
+        if match in last_words:
+            last_move_time = datetime.utcnow()
+            last_moves.append('left')
+
+            print("Found left. Attempting to execute.")
+
+            move_left(pub_controls)
+
+            return True
+
+    return False
+
+
+def back_handler(last_words, pub_controls):
+    global last_moves
+    global last_move_time
+
+    if len(last_moves) >= 1 and \
+            last_moves[-1] == 'back':
+                return []
+
+    matches = [
+        "take it back",
+        "back now y'all"
+    ]
+
+    for match in matches:
+        if match in last_words:
+            last_move_time = datetime.utcnow()
+            last_moves.append('back')
+
+            print("Found back. Attempting to execute.")
+
+            move_forward(pub_controls, -1)
+
+            return True
+
+    return False
+
+def one_hop_handler(last_words, pub_controls):
+    global last_moves
+    global last_move_time
+
+    if len(last_moves) >= 1 and \
+            last_moves[-1] == 'one hop':
+                return []
+
+    matches = [
+        "one hop this time",
+    ]
+
+    for match in matches:
+        if match in last_words:
+            last_move_time = datetime.utcnow()
+            last_moves.append('one hop')
+
+            print("Found one hop. Attempting to execute.")
+
+            move_forward(pub_controls, -1)
+
+            return True
+
+    return False
+
+def right_foot_handler(last_words, pub_controls):
+    global last_moves
+    global last_move_time
+
+    if len(last_moves) >= 1 and \
+            last_moves[-1] == 'right foot':
+                return []
+
+    matches = [
+        "right foot",
+        "to the right now",
+        "the right now",
+        "right now"
+    ]
+
+    for match in matches:
+        if match in last_words:
+            last_move_time = datetime.utcnow()
+            last_moves.append('right foot')
+
+            print("Found right foot. Attempting to execute.")
+
+            move_right(pub_controls)
+
+            return True
+
+    return False
+
+def left_foot_handler(last_words, pub_controls):
+    global last_moves
+    global last_move_time
+
+    if len(last_moves) >= 1 and \
+            last_moves[-1] == 'left foot stomp':
+                return []
+
+    matches = [
+        "left foot",
+        "left foot let's stomp",
+        "left foot lets stomp"
+    ]
+
+    for match in matches:
+        if match in last_words:
+            last_move_time = datetime.utcnow()
+            last_moves.append('left foot stomp')
+
+            print("Found left foot. Attempting to execute.")
+
+            move_left(pub_controls)
+
+            return True
+
+    return False
+
+def circle_handler(last_words, pub_controls):
+    global last_moves
+    global last_move_time
+
+    if len(last_moves) >= 1 and \
+            last_moves[-1] == 'circle':
+                return []
+
+    matches = [
+        "turn it out",
+        "turn it up"
+    ]
+
+    for match in matches:
+        if match in last_words:
+            last_move_time = datetime.utcnow()
+            last_moves.append('circle')
+
+            print("Found circle. Attempting to execute.")
+
+            move_circle(pub_controls, 1)
+
+            return True
+
+    return False
diff --git a/plan_handler.pyc b/plan_handler.pyc
new file mode 100644
index 0000000..9c9b132
Binary files /dev/null and b/plan_handler.pyc differ
diff --git a/plan_handler_test.py b/plan_handler_test.py
new file mode 100644
index 0000000..eff5de2
--- /dev/null
+++ b/plan_handler_test.py
@@ -0,0 +1,133 @@
+#!/usr/bin/env/ python
+
+import pytest
+from mock import patch
+from plan_handler import cha_cha_handler, clap_handler, left_handler, back_handler, one_hop_handler, right_foot_handler, left_foot_handler, circle_handler
+
+@patch('plan_handler.last_moves', [])
+def test_cha_cha_handler_true(mocker):
+    mocker.patch('plan_handler.move_circle', return_value=True)
+
+    res = cha_cha_handler("we're gonna get funky", None)
+
+    assert res == True
+
+@patch('plan_handler.last_moves', [])
+def test_cha_cha_handler_false(mocker):
+    mocker.patch('plan_handler.move_circle', return_value=True)
+
+    res = cha_cha_handler("NO", None)
+
+    assert res == False
+
+@patch('plan_handler.last_moves', [])
+def test_clap_handler_true(mocker):
+    mocker.patch('plan_handler.move_forward', return_value=True)
+
+    res = clap_handler("clap clap clap", None)
+
+    assert res == True
+
+@patch('plan_handler.last_moves', [])
+def test_clap_handler_false(mocker):
+    mocker.patch('plan_handler.move_forward', return_value=True)
+
+    res = clap_handler("NO", None)
+
+    assert res == False
+
+@patch('plan_handler.last_moves', [])
+def test_left_handler_true(mocker):
+    mocker.patch('plan_handler.move_left', return_value=True)
+
+    res = left_handler("to the left", None)
+
+    assert res == True
+
+@patch('plan_handler.last_moves', [])
+def test_left_handler_false(mocker):
+    mocker.patch('plan_handler.move_left', return_value=True)
+
+    res = left_handler("NO", None)
+
+    assert res == False
+
+@patch('plan_handler.last_moves', [])
+def test_back_handler_true(mocker):
+    mocker.patch('plan_handler.move_forward', return_value=True)
+
+    res = back_handler("take it back", None)
+
+    assert res == True
+
+@patch('plan_handler.last_moves', [])
+def test_back_handler_false(mocker):
+    mocker.patch('plan_handler.move_forward', return_value=True)
+
+    res = back_handler("NO", None)
+
+    assert res == False
+
+@patch('plan_handler.last_moves', [])
+def test_one_hop_handler_true(mocker):
+    mocker.patch('plan_handler.move_forward', return_value=True)
+
+    res = one_hop_handler("one hop this time", None)
+
+    assert res == True
+
+@patch('plan_handler.last_moves', [])
+def test_one_hop_handler_false(mocker):
+    mocker.patch('plan_handler.move_forward', return_value=True)
+
+    res = one_hop_handler("NO", None)
+
+    assert res == False
+
+@patch('plan_handler.last_moves', [])
+def test_right_foot_handler_true(mocker):
+    mocker.patch('plan_handler.move_right', return_value=True)
+
+    res = right_foot_handler("right foot", None)
+
+    assert res == True
+
+@patch('plan_handler.last_moves', [])
+def test_right_foot_handler_false(mocker):
+    mocker.patch('plan_handler.move_right', return_value=True)
+
+    res = right_foot_handler("NO", None)
+
+    assert res == False
+
+@patch('plan_handler.last_moves', [])
+def test_left_foot_handler_true(mocker):
+    mocker.patch('plan_handler.move_left', return_value=True)
+
+    res = left_foot_handler("left foot", None)
+
+    assert res == True
+
+@patch('plan_handler.last_moves', [])
+def test_left_foot_handler_false(mocker):
+    mocker.patch('plan_handler.move_left', return_value=True)
+
+    res = left_foot_handler("NO", None)
+
+    assert res == False
+
+@patch('plan_handler.last_moves', [])
+def test_circle_handler_true(mocker):
+    mocker.patch('plan_handler.move_circle', return_value=True)
+
+    res = circle_handler("turn it out", None)
+
+    assert res == True
+
+@patch('plan_handler.last_moves', [])
+def test_circle_handler_false(mocker):
+    mocker.patch('plan_handler.move_circle', return_value=True)
+
+    res = circle_handler("NO", None)
+
+    assert res == False
diff --git a/plan_parser.py b/plan_parser.py
new file mode 100644
index 0000000..864e11c
--- /dev/null
+++ b/plan_parser.py
@@ -0,0 +1,11 @@
+#!/usr/bin/env/ python
+
+# Import libraries
+import json
+
+
+# Parse the task planner json file into individual states.
+def parse_plan(task_planner_path):
+    with open(task_planner_path) as planner_file:
+        planner_states = json.load(planner_file)
+        return planner_states["states"]
diff --git a/plan_parser.pyc b/plan_parser.pyc
new file mode 100644
index 0000000..97b6417
Binary files /dev/null and b/plan_parser.pyc differ
diff --git a/plans/plan1.json b/plans/plan1.json
new file mode 100644
index 0000000..3efd175
--- /dev/null
+++ b/plans/plan1.json
@@ -0,0 +1,10 @@
+{
+  "states": [
+    {
+      "name": "do_forward"
+    },
+    {
+      "name": "do_backward"
+    }
+  ]
+}
diff --git a/pocketsphinx b/pocketsphinx
new file mode 160000
index 0000000..051fabb
--- /dev/null
+++ b/pocketsphinx
@@ -0,0 +1 @@
+Subproject commit 051fabb69e4439051abada8bed2c3ed3f1c35ebf
diff --git a/project2/README.md b/project2/README.md
new file mode 100644
index 0000000..e69de29
diff --git a/project2/main.py b/project2/main.py
new file mode 100644
index 0000000..e69de29
diff --git a/project2/maze_navigator.py b/project2/maze_navigator.py
new file mode 100644
index 0000000..e69de29
diff --git a/project2/moveturtle.py b/project2/moveturtle.py
new file mode 100644
index 0000000..e69de29