RickRongen/WallE
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

feat: changed movement formula

Browse Source
This commit is contained in:
Rick Rongen
2020-05-18 21:33:57 +02:00
parent 8f78b673b2
commit 0c5b9ece0c
1 changed files with 16 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
control/walle.py
View File

@@ -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: