diff --git a/rally.py b/rally.py index 6179b82..2a578f3 100644 --- a/rally.py +++ b/rally.py @@ -38,10 +38,33 @@ def find_aruco(image): return {ids[i][0]: box[0] for i, box in enumerate(corners)} + +def drunk_drive(drive_time, arlo, careful_range = 600): + 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 > careful_range and all(x > 250 for x in [right_dist, left_dist]): + turning = None + arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1) + else: + if turning == "R" or (forward_dist > careful_range and right_dist > 250 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 careful_forward(drive_time, arlo, careful_range = 600): start = time.time() - if drive_time > 2: - drive_time = 2 + if drive_time > 1: + drive_time = 1 hit_something = True else: hit_something = False @@ -92,7 +115,7 @@ def main(): if landmark in arucos: break - careful_forward(3, noah) + drunk_drive(3, noah) position = cv2.aruco.estimatePoseSingleMarkers(