46 lines
1.3 KiB
Python
46 lines
1.3 KiB
Python
import robot
|
|
import time
|
|
|
|
robot.left_distance.distance_mode = 1
|
|
robot.left_distance.start_ranging()
|
|
robot.right_distance.distance_mode = 1
|
|
robot.right_distance.start_ranging()
|
|
|
|
speed = 0.9
|
|
|
|
|
|
def speed_from_distance(distance):
|
|
limited_distance = min(distance, 30) * speed
|
|
return limited_distance / 30
|
|
|
|
|
|
try:
|
|
robot.set_left(speed)
|
|
robot.set_right(speed)
|
|
|
|
while True:
|
|
# do we have data:
|
|
if robot.left_distance.data_ready and robot.right_distance.data_ready:
|
|
left_dist = robot.left_distance.distance
|
|
right_dist = robot.right_distance.distance
|
|
l_speed = speed_from_distance(robot.right_distance.distance)
|
|
r_speed = speed_from_distance(robot.left_distance.distance)
|
|
print(
|
|
"Left: {} cm R Sp: {}, Right: {} cm L Sp: {}".format(
|
|
left_dist, r_speed, right_dist, l_speed
|
|
)
|
|
)
|
|
robot.set_left(l_speed)
|
|
robot.set_right(r_speed)
|
|
|
|
robot.left_distance.clear_interrupt()
|
|
robot.right_distance.clear_interrupt()
|
|
time.sleep(0.1)
|
|
|
|
finally:
|
|
robot.stop()
|
|
robot.left_distance.clear_interrupt()
|
|
robot.right_distance.clear_interrupt()
|
|
robot.left_distance.stop_ranging()
|
|
robot.right_distance.stop_ranging()
|