From 561b2f70a2adfd58e0f5ae873e37c3ae9965416c Mon Sep 17 00:00:00 2001 From: Pierre Tellier Date: Mon, 14 Apr 2025 18:02:30 +0200 Subject: [PATCH] fix bug in rotation --- control.py | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/control.py b/control.py index 0760fa3..2bbcca3 100644 --- a/control.py +++ b/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):