Started implementing main controller

Also sort of implemented the navigation
Also tidy up the CMakeLists a little

script/controller.py

@@ -0,0 +1,34 @@
+from time import sleep
+
+
+handlers = {}
+STATE = 'idle'  # Also walk, imitate and fallen
+
+
+def fall_handler():
+    pass
+
+
+def walk_handler():
+    pass
+
+
+def imitation_handler():
+    pass
+
+
+def handle_transition(new_state):
+    global STATE
+    if STATE == 'walk':
+        if new_state in ('fallen', 'idle'):
+            STATE = new_state
+    elif STATE == 'fallen':
+        if new_state == 'idle':
+            STATE = new_state
+    elif STATE == 'imitate':
+        STATE = new_state
+
+
+if __name__ == '__main__':
+    pass
+    # initialize stuff