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

✨

Browse Source
This commit is contained in:
NikolajDanger
2022-11-02 14:14:22 +01:00
parent 9d8e95d711
commit 21236301e0
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
rally2.py
View File

@@ -81,7 +81,7 @@ def look_around(noah, particles, cam, est_pose):
time.sleep((15 * TURN_T)/1000) time.sleep((15 * TURN_T)/1000)
noah.stop() noah.stop()
for p in particles: for p in particles:
p.setTheta(p.theta + np.deg2rad(15)) p.setTheta(p.theta - np.deg2rad(15))
# 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)
@@ -97,7 +97,7 @@ def turn_towards_landmark(noah, particles, est_pose, landmark):
angle = np.rad2deg(np.arctan(relative_pos[1]/relative_pos[0])) angle = np.rad2deg(np.arctan(relative_pos[1]/relative_pos[0]))
turn_angle = np.mod(angle - (np.rad2deg(current_theta) - 180), 360) turn_angle = np.mod(angle - (np.rad2deg(current_theta) - 180), 360)
for p in particles: for p in particles:
p.setTheta(p.theta + np.deg2rad(turn_angle)) p.setTheta(p.theta - np.deg2rad(turn_angle))
if turn_angle < 180: if turn_angle < 180:
noah.go_diff(POWER, POWER, 0, 1) noah.go_diff(POWER, POWER, 0, 1)
time.sleep((abs(turn_angle) * TURN_T)/1000) time.sleep((abs(turn_angle) * TURN_T)/1000)
@@ -273,7 +273,7 @@ def turn_90_degrees(noah, est_pose, particles):
if clockwise: if clockwise:
p.setTheta(p.theta - np.deg2rad(90)) p.setTheta(p.theta - np.deg2rad(90))
else: else:
p.setTheta(p.theta - np.deg2rad(90)) p.setTheta(p.theta + np.deg2rad(90))
particle.add_uncertainty(particles, SIGMA, SIGMA_THETA) particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
_, est_pose, particles = drive_until_stopped(noah, particles) _, est_pose, particles = drive_until_stopped(noah, particles)
return est_pose, particles return est_pose, particles