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

feat: added message for move

Browse Source
This commit is contained in:
Rick Rongen
2020-05-17 19:50:58 +02:00
parent 8b268ef48c
commit f9907ee650
5 changed files with 29 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
control/walle.py
View File

@@ -30,31 +30,30 @@ class WallE:
self.servo_controller = ServoController()
self.servo_controller.setup()
def set_movement(self, angle: float, distance: float):
if distance > 0:
distance = distance/50.0 # map 0 - 50 to 0 - 1
def set_movement(self, angle: float, force: float):
if force > 1:
force = 1.0
angle_rad = math.radians(angle)
cos_angle = math.cos(angle_rad)
sin_angle = math.sin(angle_rad)
cos_angle = math.cos(angle)
sin_angle = math.sin(angle)
velocity_l = 0
velocity_r = 0
if cos_angle > 0 and sin_angle > 0:
if cos_angle > sin_angle:
velocity_r = cos_angle * distance
velocity_l = sin_angle * distance
velocity_r = cos_angle * force
velocity_l = sin_angle * force
elif cos_angle < sin_angle:
velocity_r = sin_angle * distance
velocity_l = cos_angle * distance
velocity_r = sin_angle * force
velocity_l = cos_angle * force
elif cos_angle < 0 and sin_angle > 0:
if cos_angle < sin_angle:
velocity_r = 1 - sin_angle * distance
velocity_l = sin_angle * distance
velocity_r = 1 - sin_angle * force
velocity_l = sin_angle * force
elif cos_angle > abs(sin_angle):
velocity_r = sin_angle * distance
velocity_l = cos_angle * distance
velocity_r = sin_angle * force
velocity_l = cos_angle * force
else:
pass
self.motor_a.set(velocity_l)