NikolajDanger/Roboteksperimentarium
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

✨

Browse Source
This commit is contained in:
NikolajDanger
2022-10-26 14:43:48 +02:00
parent 04c8dcc96e
commit 15fab9c09c
1 changed files with 10 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
rally2.py
View File

@ -225,19 +225,26 @@ def main():
noah = robot.Robot() # Noah er vores robots navn noah = robot.Robot() # Noah er vores robots navn
cam = camera.Camera(0, 'arlo', useCaptureThread=True) 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: for landmark in landmark_order:
print(f"Going to landmark {landmark}") print(f"Going to landmark {landmark}")
while True: while True:
particles = initialize_particles(NUM_PARTICLES) particles = initialize_particles(NUM_PARTICLES)
# The estimate of the robots current pose # # 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) # particles, est_pose = look_around(noah, particles, cam, est_pose)
print(est_pose) print(est_pose)
particles, est_pose = turn_towards_landmark(noah, particles, est_pose, landmark) particles, est_pose = turn_towards_landmark(noah, particles, est_pose, landmark)
drive_time = time_to_landmark(est_pose, landmark) drive_time = time_to_landmark(est_pose, landmark)
if not abs(drive_until_stopped(noah) - drive_time) < 0.5: if not abs(drive_until_stopped(noah) - drive_time) < 0.5:
break
drunk_drive(noah) drunk_drive(noah)
continue continue
inch_closer(noah) inch_closer(noah)