diff --git a/rally2.py b/rally2.py index 4029dbb..fdaaa05 100644 --- a/rally2.py +++ b/rally2.py @@ -225,28 +225,22 @@ def main(): noah = robot.Robot() # Noah er vores robots navn cam = camera.Camera(0, 'arlo', useCaptureThread=True) - particles = initialize_particles(NUM_PARTICLES) - - # The estimate of the robots current pose - est_pose = particle.estimate_pose(particles) - est_pose = particle.estimate_pose(particles) - particles, est_pose = look_around(noah, particles, cam, est_pose) for landmark in landmark_order: print(f"Going to landmark {landmark}") while True: particles = initialize_particles(NUM_PARTICLES) - # # The estimate of the robots current pose - # est_pose = particle.estimate_pose(particles) - # particles, est_pose = look_around(noah, particles, cam, est_pose) + # The estimate of the robots current pose + est_pose = particle.estimate_pose(particles) + particles, est_pose = look_around(noah, particles, cam, est_pose) print(est_pose) particles, est_pose = turn_towards_landmark(noah, particles, est_pose, landmark) drive_time = time_to_landmark(est_pose, landmark) if not abs(drive_until_stopped(noah) - drive_time) < 0.5: - break drunk_drive(noah) continue + inch_closer(noah) break