From 0c5b9ece0c6cae2b121e42d18fed70199e887d62 Mon Sep 17 00:00:00 2001 From: Rick Rongen Date: Mon, 18 May 2020 21:33:57 +0200 Subject: [PATCH] feat: changed movement formula --- control/walle.py | 38 ++++++++++++++++---------------------- 1 file changed, 16 insertions(+), 22 deletions(-) diff --git a/control/walle.py b/control/walle.py index 6dc5bc2..aa86dc9 100644 --- a/control/walle.py +++ b/control/walle.py @@ -34,30 +34,24 @@ class WallE: if force > 1: force = 1.0 - cos_angle = math.cos(angle) - sin_angle = math.sin(angle) + x = math.cos(angle) * force + y = math.sin(angle) * force - velocity_l = 0 - velocity_r = 0 - - if cos_angle > 0 and sin_angle > 0: - if cos_angle > sin_angle: - velocity_r = cos_angle * force - velocity_l = sin_angle * force - elif cos_angle < sin_angle: - velocity_r = sin_angle * force - velocity_l = (1 - cos_angle) * force - elif cos_angle < 0 and sin_angle > 0: - if abs(cos_angle) < sin_angle: - velocity_r = (1 - cos_angle) * force - velocity_l = sin_angle * force - elif abs(cos_angle) > sin_angle: - velocity_r = sin_angle * force - velocity_l = cos_angle * force + if y >= 0: + n_mot_premix_l = 1.0 if x >= 0 else 1.0 + x + n_mot_premix_r = 1.0 - x if x >= 0 else 1.0 else: - pass - self.motor_a.set(velocity_l) - self.motor_b.set(velocity_r) + n_mot_premix_l = 1.0 - x if x >= 0 else 1.0 + n_mot_premix_r = 1.0 if x >= 0 else 1.0 + x + + n_mot_premix_l = n_mot_premix_l * y / 1.0 + n_mot_premix_r = n_mot_premix_r * y / 1.0 + n_piv_speed = x + f_piv_scale = 0.0 if abs(y) > .4 else 1.0 - abs(y) / .4 + mot_mix_l = (1.0 - f_piv_scale) * n_mot_premix_l + f_piv_scale * n_piv_speed + mot_mix_r = (1.0 - f_piv_scale) * n_mot_premix_r + f_piv_scale * (-n_piv_speed) + self.motor_a.set(mot_mix_l) + self.motor_b.set(mot_mix_r) def set_eye_velocity(self, angle: float, distance: float): if distance > 1: