fix bug in rotation

control.py

@@ -306,10 +306,16 @@ def dev(t, speed_x, speed_y, speed_r, y2):
     x2: rotational speed along the z-axis
     """
     max_dist = 0.3
+    print(speed_r)
 
     def get_rotation_center(speed_x, speed_y, theta_point):
-        direction = np.array([-speed_y, speed_x])
-        return direction / max(0.00001, theta_point)
+        norm = sqrt(speed_x ** 2 + speed_y ** 2)
+
+        r = 1000 if theta_point == 0 else norm / theta_point
+        if norm != 0:
+            return np.array([speed_y, -speed_x]) / norm * r
+        else:
+            return 0, 0
 
     center_x, center_y = get_rotation_center(speed_x, speed_y, speed_r)
 
@@ -337,6 +343,7 @@ def dev(t, speed_x, speed_y, speed_r, y2):
     offsets = np.array([0, 1 / 3, 2 / 3, 0, 1 / 3, 2 / 3]) * movement_duration  # offset between each leg
     assert len(offsets) == num_patte, f"all offsets must be set, currently, {len(offsets)}/{num_patte} have them"
 
+    # used to cap the speed to a max
     max_radius = 0
     for patte in range(num_patte):
         x1, y1 = real_position[patte][0], real_position[patte][1]
@@ -367,8 +374,11 @@ def dev(t, speed_x, speed_y, speed_r, y2):
 
         if y1 < center_y:
             cur_theta *= -1
+        if speed_r != 0:
+            speed = sqrt(speed_x ** 2 + speed_y ** 2) * (speed_r / abs(speed_r))
+        else:
+            speed = sqrt(speed_x ** 2 + speed_y ** 2)
 
-        speed = abs(speed_x) + abs(speed_y)
         if speed == 0:
             speed = speed_r
         dthteta = speed * (1 / max_radius * max_dist)
@@ -390,7 +400,9 @@ def dev(t, speed_x, speed_y, speed_r, y2):
         real_position[patte][0] = wanted_x
         real_position[patte][1] = wanted_y
 
-    return legs(real_position)
+    if REAL:
+        return legs(real_position)
+    return legs(real_position), [center_x, center_y]
 
 
 def jump(sy):