38 lines
1.0 KiB
Python
38 lines
1.0 KiB
Python
import time
|
|
import robot
|
|
|
|
POWER = 70
|
|
|
|
RIGHT_WHEEL_OFFSET = 5
|
|
|
|
def careful_forward(drive_time, arlo):
|
|
start = time.time()
|
|
end = start + drive_time
|
|
turning = None
|
|
while time.time() < end:
|
|
forward_dist = arlo.read_front_ping_sensor()
|
|
right_dist = arlo.read_right_ping_sensor()
|
|
left_dist = arlo.read_left_ping_sensor()
|
|
if forward_dist > 600 and all(x > 200 for x in [right_dist, left_dist]):
|
|
print("not blocked")
|
|
turning = None
|
|
arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
|
|
else:
|
|
print("blocked")
|
|
if turning == "R" or (forward_dist > 600 and right_dist > 200 and turning is None):
|
|
arlo.go_diff(POWER, POWER, 1, 0)
|
|
turning = "R"
|
|
else:
|
|
arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 0, 1)
|
|
turning = "L"
|
|
|
|
time.sleep(0.01)
|
|
|
|
def main():
|
|
drive_time = int(input())
|
|
arlo = robot.Robot()
|
|
careful_forward(drive_time, arlo)
|
|
|
|
if __name__ == "__main__":
|
|
main()
|