diff --git a/control.py b/control.py
index 5b706bb..2978071 100644
--- a/control.py
+++ b/control.py
@@ -56,12 +56,15 @@ REAL = 0
 
 # Robot setup
 if REAL == 1:
-    portHandler = PortHandler("/dev/ttyUSB1")
+    try:
+        portHandler = PortHandler("/dev/ttyUSB0")
+    except:
+        portHandler = PortHandler("/dev/ttyUSB1")
+
     packetHandler = PacketHandler(1.0)
 
     portHandler.openPort()
-    x = portHandler.setBaudRate(1000000)
-    print(x)
+    portHandler.setBaudRate(1000000)
     ADDR_GOAL_POSITION = 30
 
     ids = []
@@ -76,15 +79,19 @@ if REAL == 1:
         input()
 
 
-# Done
-
 def interpol2(point2, point1, t):
+    """
+    Linear interpolation between two points in space
+    """
     x1, y1, z1 = point1
     x2, y2, z2 = point2
     return t * x1 + (1 - t) * x2, t * y1 + (1 - t) * y2, t * z1 + (1 - t) * z2
 
 
 def get_current_step(t, step_duration, movement_duration):
+    """
+    helper function used to calculate the current step of the movement i.e. what part of the movement.
+    """
     time_passed = 0
     for i in range(len(step_duration)):
         time_passed += step_duration[i]
@@ -93,6 +100,9 @@ def get_current_step(t, step_duration, movement_duration):
 
 
 def get_current_step_advancement(t, movement_duration, step_duration, current_step):
+    """
+    helper function used to calculate the current advancement of the movement step i.e. the percentage of progression of said movement.
+    """
     current_step = get_current_step(t, step_duration, movement_duration)
     t = t % movement_duration
     for i in range(0, current_step):
@@ -102,6 +112,7 @@ def get_current_step_advancement(t, movement_duration, step_duration, current_st
 
 def inverse(x, y, z):
     """
+    Calculate the angle of each motor to have the end of the feet at a specified position
     """
     # Dimensions (m)
     z += l1v
@@ -109,7 +120,6 @@ def inverse(x, y, z):
     theta0 = atan2(y, x)
 
     l = sqrt((sqrt(x ** 2 + y ** 2) - l1) ** 2 + z ** 2)
-    # l = sqrt((x - l1h*cos(theta0)) ** 2 + (y - l1h*sin(theta0)) ** 2 + (z + l1v) ** 2)
 
     param2 = -1 * (-(l ** 2) + l2 ** 2 + l3 ** 2) / (2 * l2 * l3)
 
@@ -126,15 +136,13 @@ def inverse(x, y, z):
 
     theta1 = acos(param1) + asin(z / l)
 
-    # return [-theta0, theta1, theta2]
     angle1 = atan(l2v / l2h)
     return [-theta0, theta1 + angle1, theta2 + angle1 - pi / 2 + atan(l3h / l3v)]
-    # return [0, angle1 , angle1 -pi/2 + atan(l3h/l3v)]
 
 
 def legs(targets_robot):
     """
-    takes a list of target and offsets it to be in the legs referential
+    takes a list of target and offsets them to be in the legs referential
     """
 
     targets = [0] * 18
@@ -163,7 +171,72 @@ def legs(targets_robot):
     return targets
 
 
+def normalize(matrix, slider_max, speed):
+    return (matrix / slider_max) * speed
+
+
+counter = 0
+
+
+def convert_to_robot(positions):
+    """
+    Send a robot position to the real robot.
+    """
+    global counter
+    counter += 1
+    if counter % 2 != 0:
+        return
+    index = [
+        11, 12, 13,
+        41, 42, 43,
+        21, 22, 23,
+        51, 52, 53,
+        61, 62, 63,
+        31, 32, 33]
+    dir = [-1] * 18
+
+    for i in range(len(positions)):
+        value = int(512 + positions[i] * 150 * dir[i])
+        packetHandler.write2ByteTxOnly(portHandler, index[i], ADDR_GOAL_POSITION, value)
+
+
+def walk(t, sx, sy, sr):
+    """
+    Choose the algorithm depending on input of xbox controller
+    """
+    xboxdata = xbox.get_data()
+    if xbox.initialized:
+        max_slider = 0.200
+        controllerLogger.debug(xboxdata)
+        if xbox.mode == 0:
+            positions = static()
+        elif xbox.mode == 1:
+            positions = jump(xboxdata["y2"])
+        elif xbox.mode == 2:
+            positions = dino_naive(t, max_slider * xboxdata["y1"], max_slider * xboxdata["x1"],
+                                   max_slider * xboxdata["y2"],
+                                   max_slider * xboxdata["x2"], max_slider * (xboxdata["r2"] + 1))
+        elif xbox.mode == 3:
+            positions = naive_walk(t, max_slider * xboxdata["x1"], max_slider * xboxdata["y1"])
+        elif xbox.mode == 4:
+            positions = walk_v2(t, max_slider * xboxdata["y1"], -1 * max_slider * xboxdata["x1"],
+                                xboxdata["x2"] * max_slider,
+                                max_slider * xboxdata["y2"])
+        # print(positions)
+        if REAL == 1:
+            convert_to_robot(positions)
+
+        return positions
+    else:
+        return walk_v2(t, sx, sy, sr, None)
+
+
+# different move algorithm
+
 def naive_walk(t, speed_x, speed_y):
+    """
+    First version of walk, using triangle for each step
+    """
     slider_max = 0.200
 
     real_position = np.copy(neutral_position)
@@ -229,88 +302,25 @@ def naive_walk(t, speed_x, speed_y):
     return legs(real_position)
 
 
-def normalize(matrix, slider_max, speed):
-    return (matrix / slider_max) * speed
-
-
-counter = 0
-
-
-def convert_to_robot(positions):
-    global counter
-    counter += 1
-    if counter % 2 != 0:
-        return
-    index = [
-        11, 12, 13,
-        41, 42, 43,
-        21, 22, 23,
-        51, 52, 53,
-        61, 62, 63,
-        31, 32, 33]
-    dir = [-1] * 18
-
-    for i in range(len(positions)):
-        value = int(512 + positions[i] * 150 * dir[i])
-        packetHandler.write2ByteTxOnly(portHandler, index[i], ADDR_GOAL_POSITION, value)
-
-
-def stand(x1, y1, x2, y2):
-    return [
-        0, 0, 0,
-        0, 0, 0,
-        x1, 0, 0,
-        0, -0.5, 0,
-        0, -0.5, 0,
-        y1, 0, 0
-    ]
-
-
-def walk(t, sx, sy, sr):
-    xboxdata = xbox.get_data()
-    if xbox.initialized:
-        max_slider = 0.200
-        controllerLogger.debug(xboxdata)
-        if xbox.mode == 0:
-            positions = static()
-        elif xbox.mode == 1:
-            positions = jump(xboxdata["y2"])
-        elif xbox.mode == 2:
-            positions = dino_naive(t, max_slider * xboxdata["y1"], max_slider * xboxdata["x1"],
-                                   max_slider * xboxdata["y2"],
-                                   max_slider * xboxdata["x2"], max_slider * (xboxdata["r2"] + 1))
-        elif xbox.mode == 3:
-            positions = naive_walk(t, max_slider * xboxdata["x1"], max_slider * xboxdata["y1"])
-        elif xbox.mode == 4:
-            positions = walk_v2(t, max_slider * xboxdata["y1"], -1 * max_slider * xboxdata["x1"],
-                                xboxdata["x2"] * max_slider,
-                                max_slider * xboxdata["y2"])
-        elif xbox.mode == 5:
-            positions = stand(xboxdata["x1"], xboxdata["y1"], xboxdata["x2"], xboxdata["y2"])
-
-        # print(positions)
-        if REAL == 1:
-            convert_to_robot(positions)
-
-        return positions
-    else:
-        return walk_v2(t, sx, sy, sr, None)
-
-
 def static():
+    """
+    Function to have the robot stand at the neutral position
+    """
     return legs(neutral_position)
 
 
 # Walk V2
 def walk_v2(t, speed_x, speed_y, speed_r, y2):
     """
-    x1: speed along the x-axis
-    y1: speed along the y-axis
-    x2: rotational speed along the z-axis
+    Second version of the walking algorithm, using circle for each leg
+    This is the main algorithm.
     """
     max_dist = 0.3
 
     def get_rotation_center(speed_x, speed_y, theta_point):
+        """
+        Helper function that calculate the center of rotation of the robot based on the current parameters
+        """
         norm = sqrt(speed_x ** 2 + speed_y ** 2)
 
         r = 1000 if theta_point == 0 else norm / theta_point
@@ -320,9 +330,7 @@ def walk_v2(t, speed_x, speed_y, speed_r, y2):
             return 0, 0
 
     center_x, center_y = get_rotation_center(speed_x, speed_y, speed_r)
-
     legsLogger.debug(f"rotation center: center_x: {center_x}, center_y: {center_y}")
-
     real_position = np.copy(neutral_position)
 
     movement_z = np.array([
@@ -408,6 +416,9 @@ def walk_v2(t, speed_x, speed_y, speed_r, y2):
 
 
 def jump(sy):
+    """
+    Make the robot jump (at least we wished)
+    """
     offset = np.array([
         [0, 0, -0.15],
         [0, 0, -0.15],
@@ -420,8 +431,10 @@ def jump(sy):
     return legs(neutral_position + offset)
 
 
-# based on walk V1
 def dino_naive(t, speed_x, speed_y, hz, hy, hx):
+    """
+    based on walk naive, but with a head and a tail
+    """
     slider_max = 0.200
 
     real_position = np.copy(neutral_position)