diff --git a/rally.py b/rally.py index 18c7ab0..4692a77 100644 --- a/rally.py +++ b/rally.py @@ -72,38 +72,40 @@ def main(): landmark = landmark_order.pop(0) while True: - for _ in range(24): - arucos = find_aruco(noah.take_photo()) + while True: + for _ in range(24): + arucos = find_aruco(noah.take_photo()) + if landmark in arucos: + break + noah.go_diff(POWER, POWER, 0, 1) + time.sleep((20 * TURN_T)/1000) + noah.stop() + if landmark in arucos: break - noah.go_diff(POWER, POWER, 0, 1) - time.sleep((20 * TURN_T)/1000) - noah.stop() - if landmark in arucos: + careful_forward(5, noah) + + + position = cv2.aruco.estimatePoseSingleMarkers( + np.array([arucos[landmark]]), 14.5, CAMERA_MATRIX, DIST_COEF + )[1][0][0] + + angle = np.rad2deg(np.arctan(position[0]/position[2])) + drive_distance = np.sqrt(position[0]**2 + position[2]**2) - 10 + if drive_distance < 30: break - careful_forward(5, noah) + if angle < 0: + noah.go_diff(POWER, POWER, 0, 1) + time.sleep((abs(angle) * TURN_T)/1000) + noah.stop() + else: + noah.go_diff(POWER, POWER, 1, 0) + time.sleep((abs(angle) * TURN_T * CLOCKWISE_OFFSET)/1000) + noah.stop() - - position = cv2.aruco.estimatePoseSingleMarkers( - np.array([arucos[landmark]]), 14.5, CAMERA_MATRIX, DIST_COEF - )[1][0][0] - - angle = np.rad2deg(np.arctan(position[0]/position[2])) - drive_distance = np.sqrt(position[0]**2 + position[2]**2) - 10 - if angle < 0: - noah.go_diff(POWER, POWER, 0, 1) - time.sleep((abs(angle) * TURN_T)/1000) - noah.stop() - else: - noah.go_diff(POWER, POWER, 1, 0) - time.sleep((abs(angle) * TURN_T * CLOCKWISE_OFFSET)/1000) - noah.stop() - - noah.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1) - time.sleep((drive_distance * DRIVE_T)/1000) - noah.stop() + careful_forward((drive_distance * DRIVE_T)/1000) if __name__ == "__main__": main()